diff options
Diffstat (limited to 'src')
71 files changed, 3891 insertions, 506 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 41942aacd..293690d27 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -165,7 +165,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 2; + _retries = 0; return ret; } @@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg) Airspeed *dev = (Airspeed *)arg; dev->cycle(); - dev->update_status(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); } void diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 84815fdfb..5aff6825b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -95,6 +95,11 @@ __BEGIN_DECLS #define PWM_LOWEST_MAX 1700 /** + * Do not output a channel with this value + */ +#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX + +/** * Servo output signal type, value is actual servo output pulse * width in microseconds. */ diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e..ab640837b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index f98d615a2..0f77bb805 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -155,7 +155,6 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - uint16_t diff_pres_pa; if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the @@ -166,28 +165,21 @@ ETSAirspeed::collect() return -1; } - if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { - diff_pres_pa = 0; - } else { - diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; - } - // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa_raw; } differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = (float)diff_pres_pa; // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = (float)diff_pres_pa; - report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; + report.differential_pressure_filtered_pa = diff_pres_pa_raw; + report.differential_pressure_raw_pa = diff_pres_pa_raw; report.temperature = -1000.0f; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -369,7 +361,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -394,7 +386,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f17e99e9d..f0dc0c651 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -392,7 +392,8 @@ HIL::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0e9a961ac..e4ecfa6b5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); + _conf_reg &= ~0x03; // reset previous excitement mode if (((int)enable) < 0) { _conf_reg |= 0x01; } else if (enable > 0) { _conf_reg |= 0x02; - } else { - _conf_reg &= ~0x03; } // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 159706278..1d9a463ad 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,44 +228,23 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - float diff_press_pa = fabsf(diff_press_pa_raw); - /* - note that we return both the absolute value with offset - applied and a raw value without the offset applied. This - makes it possible for higher level code to detect if the - user has the tubes connected backwards, and also makes it - possible to correctly use offsets calculated by a higher - level airspeed driver. - With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port - - Also note that the _diff_pres_offset is applied before the - fabsf() not afterwards. It needs to be done this way to - prevent a bias at low speeds, but this also means that when - setting a offset you must set it based on the raw value, not - the offset value */ - + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ - if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; - report.differential_pressure_pa = diff_press_pa; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); - - /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -345,7 +324,7 @@ MEASAirspeed::cycle() /** correct for 5V rail voltage if the system_power ORB topic is available - + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of offset versus voltage for 3 sensors */ @@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } - temperature -= voltage_diff * temp_slope; + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } @@ -523,7 +502,7 @@ test() } warnx("single read"); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -551,7 +530,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 873fa62c4..889643d0d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -130,7 +130,7 @@ protected: float _T; /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ + unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; @@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + irqrestore(flags); return OK; } diff --git a/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 000000000..9c5eb42eb --- /dev/null +++ b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/pca9685/module.mk b/src/drivers/pca9685/module.mk new file mode 100644 index 000000000..7a5c7996e --- /dev/null +++ b/src/drivers/pca9685/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Driver for the PCA9685 I2C PWM controller +# The chip is used on the adafruit I2C PWM converter, +# which allows to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = pca9685 + +SRCS = pca9685.cpp diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp new file mode 100644 index 000000000..6f69ce8a1 --- /dev/null +++ b/src/drivers/pca9685/pca9685.cpp @@ -0,0 +1,651 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pca9685.cpp + * + * Driver for the PCA9685 I2C PWM module + * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815 + * + * Parts of the code are adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> +#include <math.h> + +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <uORB/uORB.h> +#include <uORB/topics/actuator_controls.h> + +#include <board_config.h> +#include <drivers/drv_io_expander.h> + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OF + +#define ADDR 0x40 // I2C adress + +#define PCA9685_DEVICE_PATH "/dev/pca9685" +#define PCA9685_BUS PX4_I2C_BUS_EXPANSION +#define PCA9685_PWMFREQ 60.0f +#define PCA9685_NCHANS 16 // total amount of pwm outputs + +#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2) +#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees + PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG + PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG + */ +#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +class PCA9685 : public device::I2C +{ +public: + PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + virtual ~PCA9685(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int info(); + virtual int reset(); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; + + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + +}; + +/* for now, we only support one board */ +namespace +{ +PCA9685 *g_pca9685; +} + +void pca9685_usage(); + +extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); + +PCA9685::PCA9685(int bus, uint8_t address) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) +{ + memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); +} + +PCA9685::~PCA9685() +{ +} + +int +PCA9685::init() +{ + int ret; + ret = I2C::init(); + if (ret != OK) { + return ret; + } + + ret = reset(); + if (ret != OK) { + return ret; + } + + ret = setPWMFreq(PCA9685_PWMFREQ); + + return ret; +} + +int +PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + switch (cmd) { + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + warnx("shutting down"); + break; + case IOX_MODE_ON: + warnx("starting"); + break; + case IOX_MODE_TEST_OUT: + warnx("test starting"); + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1); + } + + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +int +PCA9685::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + +void +PCA9685::i2cpwm_trampoline(void *arg) +{ + PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +PCA9685::i2cpwm() +{ + if (_mode == IOX_MODE_TEST_OUT) { + setPin(0, PCA9685_PWMCENTER); + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); + + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + /* Scale the controls to PWM, first multiply by pi to get rad, + * the control[i] values are on the range -1 ... 1 */ + uint16_t new_value = PCA9685_PWMCENTER + + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } + _should_run = true; + } + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +int +PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) +{ + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +PCA9685::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +PCA9685::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +PCA9685::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int PCA9685::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +PCA9685::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + +void +pca9685_usage() +{ + warnx("missing command: try 'start', 'test', 'stop', 'info'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca9685_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca9685_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca9685_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca9685 != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_pca9685 != nullptr && OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + } + + if (g_pca9685 == nullptr) { + errx(1, "init failed"); + } + } + + if (g_pca9685 == nullptr) { + g_pca9685 = new PCA9685(i2cdevice, i2caddr); + + if (g_pca9685 == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + errx(1, "init failed"); + } + } + fd = open(PCA9685_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + + + exit(0); + } + + // need the driver past this point + if (g_pca9685 == nullptr) { + warnx("not started, run pca9685 start"); + exit(1); + } + + if (!strcmp(verb, "info")) { + g_pca9685->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_pca9685->reset(); + exit(0); + } + + + if (!strcmp(verb, "test")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca9685->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca9685->is_running()) { + delete g_pca9685; + g_pca9685= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + pca9685_usage(); + exit(0); +} diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964..60ad3c1af 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -68,7 +68,7 @@ #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> -//#include <uORB/topics/optical_flow.h> +#include <uORB/topics/optical_flow.h> #include <board_config.h> @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 82977a032..122a3cd17 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); + if (values[i] != PWM_IGNORE_THIS_CHANNEL) { + up_pwm_servo_set(i, values[i]); + } } return count * 2; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d93009c47..97919538f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -136,6 +136,15 @@ public: virtual int init(); /** + * Initialize the PX4IO class. + * + * Retrieve relevant initial system parameters. Initialize PX4IO registers. + * + * @param disable_rc_handling set to true to forbid override / RC handling on IO + */ + int init(bool disable_rc_handling); + + /** * Detect if a PX4IO is connected. * * Only validate if there is a PX4IO to talk to. @@ -580,6 +589,12 @@ PX4IO::detect() } int +PX4IO::init(bool rc_handling_disabled) { + _rc_handling_disabled = rc_handling_disabled; + return init(); +} + +int PX4IO::init() { int ret; @@ -778,6 +793,11 @@ PX4IO::init() if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + if (ret != OK) { + log("failed disabling RC handling"); + return ret; + } + } else { /* publish RC config to IO */ ret = io_set_rc_config(); @@ -1175,6 +1195,7 @@ PX4IO::io_set_arming_state() int PX4IO::disable_rc_handling() { + _rc_handling_disabled = true; return io_disable_rc_handling(); } @@ -1947,7 +1968,7 @@ PX4IO::print_status(bool extended_status) printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); printf("servos"); @@ -2613,24 +2634,25 @@ start(int argc, char *argv[]) errx(1, "driver alloc failed"); } - if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); - } + bool rc_handling_disabled = false; /* disable RC handling on request */ if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if (g_dev->disable_rc_handling()) - warnx("Failed disabling RC handling"); + rc_handling_disabled = true; } else { warnx("unknown argument: %s", argv[1]); } } + if (OK != g_dev->init(rc_handling_disabled)) { + delete g_dev; + g_dev = nullptr; + errx(1, "driver init failed"); + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bca1715fa..80ecab2ee 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -598,7 +598,8 @@ SF0X::collect() memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } - if (_linebuf[i] == '.') { + /* we need a digit before the dot and a dot for a valid number */ + if (i > 0 && _linebuf[i] == '.') { valid = true; } } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6..926a8db2a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d7..94bd26f03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e600976ce..1c8d2a2a7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,39 +49,124 @@ #include <stdio.h> #include <math.h> #include <stdbool.h> +#include <string.h> +#include <float.h> + +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> /* * Azimuthal Equidistant Projection * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0}; +static struct globallocal_converter_reference_s gl_ref = {0.0f, false}; + +__EXPORT bool map_projection_global_initialized() +{ + return map_projection_initialized(&mp_ref); +} + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__EXPORT uint64_t map_projection_global_timestamp() +{ + return map_projection_timestamp(&mp_ref); +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) { - ref->lat = lat_0 / 180.0 * M_PI; - ref->lon = lon_0 / 180.0 * M_PI; + return ref->timestamp; +} - ref->sin_lat = sin(ref->lat); - ref->cos_lat = cos(ref->lat); +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + if (strcmp("commander", getprogname()) == 0) { + return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); + } else { + return -1; + } } -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { - double lat_rad = lat / 180.0 * M_PI; - double lon_rad = lon / 180.0 * M_PI; + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); +} + +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad) +{ + return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y) +{ + return map_projection_project(&mp_ref, lat, lon, x, y); + +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; double sin_lat = sin(lat_rad); double cos_lat = cos(lat_rad); - double cos_d_lon = cos(lon_rad - ref->lon); + double cos_d_lon = cos(lon_rad - ref->lon_rad); double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); - double k = (c == 0.0) ? 1.0 : (c / sin(c)); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; - *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon) +{ + return map_projection_reproject(&mp_ref, x, y, lat, lon); } -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { + if (!map_projection_initialized(ref)) { + return -1; + } + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); @@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lat_rad; double lon_rad; - if (c != 0.0) { + if (fabs(c) > DBL_EPSILON) { lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); - lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); } else { - lat_rad = ref->lat; - lon_rad = ref->lon; + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; } *lat = lat_rad * 180.0 / M_PI; *lon = lon_rad * 180.0 / M_PI; + + return 0; +} + +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (lat_0 != NULL) { + *lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad; + } + + if (lon_0 != NULL) { + *lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad; + } + + return 0; + +} +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp) +{ + if (strcmp("commander", getprogname()) == 0) { + gl_ref.alt = alt_0; + if (!map_projection_global_init(lat_0, lon_0, timestamp)) + { + gl_ref.init_done = true; + return 0; + } else { + gl_ref.init_done = false; + return -1; + } + } else { + return -1; + } +} + +__EXPORT bool globallocalconverter_initialized() +{ + return gl_ref.init_done && map_projection_global_initialized(); } +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_project(lat, lon, x, y); + *z = gl_ref.alt - alt; + + return 0; +} + +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + map_projection_global_reproject(x, y, lat, lon); + *alt = gl_ref.alt - z; + + return 0; +} + +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0) +{ + if (!map_projection_global_initialized()) { + return -1; + } + + if (map_projection_global_getref(lat_0, lon_0)) + { + return -1; + } + + if (alt_0 != NULL) { + *alt_0 = gl_ref.alt; + } + + return 0; +} __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8b286af36..2311e0a7c 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -69,39 +69,162 @@ struct crosstrack_error_s { /* lat/lon are in radians */ struct map_projection_reference_s { - double lat; - double lon; + double lat_rad; + double lon_rad; double sin_lat; double cos_lat; + bool init_done; + uint64_t timestamp; }; +struct globallocal_converter_reference_s { + float alt; + bool init_done; +}; + +/** + * Checks if global projection was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool map_projection_global_initialized(void); + +/** + * Checks if projection given as argument was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref); + +/** + * Get the timestamp of the global map projection + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_global_timestamp(void); + +/** + * Get the timestamp of the map projection given by the argument + * @return the timestamp of the map_projection + */ +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref); + +/** + * Writes the reference values of the global projection to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad); + /** - * Initializes the map transformation. + * Writes the reference values of the projection given by the argument to ref_lat and ref_lon + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); + +/** + * Initializes the global map transformation. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument. + * + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, + double lat_0, double lon_0, uint64_t timestamp); + +/** + * Initializes the map transformation given by the argument and sets the timestamp to now. * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * Initializes the transformation between the geographic coordinate system and + * the azimuthal equidistant plane * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ -__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0); /** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the global projection * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); + + + /* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); + +/** + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the global projection + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon); /** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * Transforms a point in the local azimuthal equidistant plane to the + * geographic coordinate system using the projection given by the argument * * @param x north * @param y east * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) + * @return 0 if map_projection_init was called before, -1 else + */ +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); + +/** + * Get reference position of the global map projection + */ +__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0); + +/** + * Initialize the global mapping between global position (spherical) and local position (NED). + */ +__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp); + +/** + * Checks if globallocalconverter was initialized + * @return true if map was initialized before, false else + */ +__EXPORT bool globallocalconverter_initialized(void); + +/** + * Convert from global position coordinates to local position coordinates using the global reference + */ +__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z); + +/** + * Convert from local position coordinates to global position coordinates using the global reference + */ +__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt); + +/** + * Get reference position of the global to local converter */ -__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0); /** * Returns the distance to the next waypoint in meters. diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a69..9d479832f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x) last_timestamp = hrt_absolute_time(); if (accel_x > threshold_accel.get()) { - integrator += accel_x * dt; + integrator += dt; // warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_accel.get() * threshold_time.get()) { + if (integrator > threshold_time.get()) { launchDetected = true; } diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp new file mode 100644 index 000000000..de3f20a95 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -0,0 +1,893 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop.cpp + * + * Bottle drop module for Outback Challenge 2014, Team Swiss Fang + * + * @author Dominik Juchli <juchlid@ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <sys/ioctl.h> +#include <drivers/device/device.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/wind_estimate.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <dataman/dataman.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + + void open_bay(); + void close_bay(); + void drop(); + void lock_release(); + +private: + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ + int _mavlink_fd; + + int _command_sub; + int _wind_estimate_sub; + struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _drop_approval; + hrt_abstime _doors_opened; + hrt_abstime _drop_time; + + float _alt_clearance; + + struct position_s { + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m + } _target_position, _drop_position; + + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Set the actuators + */ + int actuators_publish(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1), + _command_sub(-1), + _wind_estimate_sub(-1), + _command {}, + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(70.0f), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("drop state: %d", _drop_state); +} + +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); + + usleep(500 * 1000); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + _actuators.control[2] = 1.0f; + + _drop_time = hrt_absolute_time(); + actuators_publish(); + + warnx("dropping now"); + + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; + actuators_publish(); + + warnx("closing release"); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + +void +BottleDrop::task_main() +{ + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + float ground_distance = _alt_clearance; // Replace by closer estimate in loop + + // constant + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + unsigned counter = 0; + + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &m); + param_get(param_surface, &A); + + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + struct wind_estimate_s wind; + + // wakeup source(s) + struct pollfd fds[1]; + + // Setup of loop + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + // Whatever state the bay is in, we want it closed on startup + lock_release(); + close_bay(); + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + if (_global_pos.timestamp == 0) { + continue; + } + + const unsigned sleeptime_us = 50000; + + hrt_abstime last_run = hrt_absolute_time(); + float dt_runs = 1e6f / sleeptime_us; + + // switch to faster updates during the drop + while (_drop_state > DROP_STATE_INIT) { + + // Get wind estimate + orb_check(_wind_estimate_sub, &updated); + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } + + // Get vehicle position + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + // Get parameter updates + orb_check(parameter_update_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + // update all parameters + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + ground_distance = _global_pos.alt - _target_position.alt; + + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + } + + switch (_drop_state) { + + case DROP_STATE_TARGET_VALID: + { + + // Update drop point at 10 Hz + if (counter % 10 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // compute drop vector + x = groundspeed_body * t_signal + x; + } + + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = _target_position.alt + _alt_clearance; + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt; + + // Save WPs in datamanager + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + + } else { + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); + } + + _drop_state = DROP_STATE_TARGET_SET; + } + break; + + case DROP_STATE_TARGET_SET: + { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } + } + } + break; + + case DROP_STATE_BAY_OPEN: + { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + + warnx("Distance real: %.2f", (double)distance_real); + + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance) || + (distance_real < precision / 10.0f))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + } + } + } + break; + + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + break; + } + + counter++; + + // update_actuators(); + + // run at roughly 20 Hz + usleep(sleeptime_us); + + dt_runs = 1e6f / hrt_elapsed_time(&last_run); + last_run = hrt_absolute_time(); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) +{ + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + open_bay(); + drop(); + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + + } else if (cmd->param1 > 0.5f) { + open_bay(); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + + } else { + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + break; + + default: + _drop_approval = false; + warnx("param1 val unknown"); + break; + } + + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + _drop_state = DROP_STATE_TARGET_VALID; + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); + map_projection_init(&ref, _target_position.lat, _target_position.lon); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + + if (cmd->param1 < 0) { + + // Clear internal states + _drop_approval = false; + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + _onboard_mission.current_seq = -1; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + default: + break; + } +} + +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} + +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) +{ + bottle_drop::g_bottle_drop->task_main(); +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "not running"); + } + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 000000000..e5d35bf0a --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop_params.c + * Bottle drop parameters + * + * @author Dominik Juchli <juchlid@ethz.ch> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 000000000..6b18fff55 --- /dev/null +++ b/src/modules/bottle_drop/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.cpp \ + bottle_drop_params.c diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..339b11bbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4f976546e..a5a772825 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -59,6 +59,7 @@ #include <string.h> #include <math.h> #include <poll.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -92,6 +93,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <geo/geo.h> #include <systemlib/state_table.h> #include <dataman/dataman.h> @@ -127,7 +129,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 +#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -613,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; @@ -875,6 +882,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -1335,6 +1344,16 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + } + + /* start mission result check */ orb_check(mission_result_sub, &updated); if (updated) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f8589d24b..684c61a17 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f))) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } } } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4..ad873203e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -145,6 +145,7 @@ private: perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), + _debug(false) { /* safely initialize structs */ _att = {}; @@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main() perf_count(_nonfinite_input_perf); } } else { - airspeed = _airspeed.true_airspeed_m_s; + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* @@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } @@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } @@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } @@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main() /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk index 3b8553489..70c75b10c 100644 --- a/src/modules/gpio_led/module.mk +++ b/src/modules/gpio_led/module.mk @@ -37,3 +37,5 @@ MODULE_COMMAND = gpio_led SRCS = gpio_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 17d1babff..5b65dc369 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -39,6 +39,9 @@ #include "mavlink_ftp.h" +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG + MavlinkFTP *MavlinkFTP::_server; MavlinkFTP * @@ -111,7 +114,7 @@ MavlinkFTP::_worker(Request *req) uint32_t messageCRC; // basic sanity checks; must validate length before use - if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + if (hdr->size > kMaxDataLength) { errorCode = kErrNoRequest; goto out; } @@ -119,13 +122,18 @@ MavlinkFTP::_worker(Request *req) // check request CRC to make sure this is one of ours messageCRC = hdr->crc32; hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; warnx("ftp: bad crc"); } - //printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); +#endif switch (hdr->opcode) { case kCmdNone: @@ -172,7 +180,9 @@ out: // handle success vs. error if (errorCode == kErrNone) { hdr->opcode = kRspAck; - //warnx("FTP: ack\n"); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: ack\n"); +#endif } else { warnx("FTP: nak %u", errorCode); hdr->opcode = kRspNak; @@ -192,10 +202,13 @@ MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); - hdr->magic = kProtocolMagic; + hdr->seqNumber = req->header()->seqNumber + 1; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source @@ -217,7 +230,9 @@ MavlinkFTP::_workList(Request *req) return kErrNotDir; } - //warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: list %s offset %d", dirPath, hdr->offset); +#endif ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; @@ -247,11 +262,13 @@ MavlinkFTP::_workList(Request *req) uint32_t fileSize = 0; char buf[256]; + char direntType; - // store the type marker + // Determine the directory entry type switch (entry.d_type) { case DTYPE_FILE: - hdr->data[offset++] = kDirentFile; + // For files we get the file size as well + direntType = kDirentFile; snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); struct stat st; if (stat(buf, &st) == 0) { @@ -259,29 +276,34 @@ MavlinkFTP::_workList(Request *req) } break; case DTYPE_DIRECTORY: - hdr->data[offset++] = kDirentDir; + direntType = kDirentDir; break; default: - hdr->data[offset++] = kDirentUnknown; + direntType = kDirentUnknown; break; } if (entry.d_type == DTYPE_FILE) { + // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { + // Everything else just sends name strncpy(buf, entry.d_name, sizeof(buf)); buf[sizeof(buf)-1] = 0; } size_t nameLen = strlen(buf); - // name too big to fit? - if ((nameLen + offset + 2) > kMaxDataLength) { + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { break; } - // copy the name, which we know will fit + // Move the data into the buffer + hdr->data[offset++] = direntType; strcpy((char *)&hdr->data[offset], buf); - //printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#ifdef MAVLINK_FTP_DEBUG + printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); +#endif offset += nameLen + 1; } @@ -342,7 +364,9 @@ MavlinkFTP::_workRead(Request *req) } // Seek to the specified position - //warnx("seek %d", hdr->offset); +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", hdr->offset); +#endif if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 800c98b69..1dd8f102e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -38,9 +38,6 @@ * * MAVLink remote file server. * - * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes - * a session ID and sequence number. - * * A limited number of requests (currently 2) may be outstanding at a time. * Additional messages will be discarded. * @@ -74,16 +71,19 @@ private: static MavlinkFTP *_server; + /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the + /// structure ourselves to 32 bit alignment which should get us what we want. struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; + { + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; - }; + }; enum Opcode : uint8_t { @@ -131,10 +131,11 @@ private: }; bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + _systemId = fromMessage->sysid; _mavlink = mavlink; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - return true; + mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); + return _message.target_system == _mavlink->get_system_id(); } return false; } @@ -145,8 +146,14 @@ private: // flat memory architecture, as we're operating between threads here. mavlink_message_t msg; msg.checksum = 0; - unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence()+1, rawData()); + unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id + _mavlink->get_component_id(), // Sender component id + _mavlink->get_channel(), // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + _systemId, // Target system id + 0, // Target component id + rawData()); // Payload to pack into message _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); @@ -154,37 +161,38 @@ private: if (!success) { warnx("FTP TX ERR"); - } - // else { - // warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - // _mavlink->get_system_id(), - // _mavlink->get_component_id(), - // _mavlink->get_channel(), - // len, - // msg.checksum); - // } + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } +#endif } - uint8_t *rawData() { return &_message.data[0]; } - RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); } + uint8_t *rawData() { return &_message.payload[0]; } + RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); } uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - uint16_t sequence() const { return _message.seqnr; } mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: Mavlink *_mavlink; - mavlink_encapsulated_data_t _message; + mavlink_file_transfer_protocol_t _message; + uint8_t _systemId; }; - static const uint8_t kProtocolMagic = 'f'; static const char kDirentFile = 'F'; static const char kDirentDir = 'D'; static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); /// Request worker; runs on the low-priority work queue to service /// remote requests. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a39d3bed..dd830071d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,44 +134,44 @@ Mavlink::Mavlink() : _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(1000), - _datarate_events(500), - _rate_mult(1.0f), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _last_write_success_time(0), - _last_write_try_time(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _send_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -217,6 +217,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() @@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { - _mode = MAVLINK_MODE_CAMERA; + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; } break; @@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; - case MAVLINK_MODE_CAMERA: - warnx("mode: CAMERA"); + case MAVLINK_MODE_ONBOARD: + warnx("mode: ONBOARD"); break; default: @@ -1382,22 +1387,26 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("ATTITUDE", 15.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f); - configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 15.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; - case MAVLINK_MODE_CAMERA: + case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("GLOBAL_POSITION_INT", 15.0f); - configure_stream("CAMERA_CAPTURE", 1.0f); + // XXX OBC change back: We need to be bandwidth-efficient here too + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("ATTITUDE_TARGET", 50.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); break; default: @@ -1653,6 +1662,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1e2e94cef..7f9d225bb 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -127,7 +127,7 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_CAMERA + MAVLINK_MODE_ONBOARD }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c10be77b5..a2f3828ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -774,6 +774,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_combined_sub; + uint64_t _sensor_combined_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -789,7 +792,9 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_combined_time(0) {} void send(const hrt_abstime t) @@ -799,12 +804,14 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; + struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -813,7 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); @@ -879,8 +886,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -950,11 +957,11 @@ protected: msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; msg.alt = pos.alt * 1000.0f; - msg.relative_alt = (pos.alt - home.alt) * 1000.0f; - msg.vx = pos.vel_n * 100.0f; - msg.vy = pos.vel_e * 100.0f; - msg.vz = pos.vel_d * 100.0f; - msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } @@ -1015,9 +1022,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } @@ -1078,9 +1085,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } @@ -1333,7 +1340,7 @@ protected: } for (unsigned i = 0; i < 8; i++) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (act.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); @@ -1344,7 +1351,7 @@ protected: } } else { - /* send 0 when disarmed */ + /* send 0 when disarmed and for disabled channels */ out[i] = 0.0f; } } @@ -1353,15 +1360,20 @@ protected: /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale PWM out 900..2100 us to -1..1 for normal channels */ - out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + if (act.output[i] > PWM_LOWEST_MIN / 2) { + if (i != 3) { + /* scale PWM out 900..2100 us to -1..1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + /* scale PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } } else { - /* scale PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + /* set 0 for disabled channels */ + out[i] = 0.0f; } - } } @@ -1637,10 +1649,10 @@ protected: msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX; msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX; msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; - msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; + msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX; + msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX; + msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX; + msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX; msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7a761588a..d436c95e9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,6 +58,10 @@ #endif static const int ERROR = -1; +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(_interval / 10.0f), - _verbose(false), - _comp_id(MAV_COMP_ID_MISSIONPLANNER) + _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + if (CHECK_SYSID_COMPID_MISSION(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 8ff27f87d..a7bb94c22 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -126,8 +126,6 @@ private: bool _verbose; - uint8_t _comp_id; - /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 63bac45c0..bed1bd789 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,10 +108,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp_pub(-1), _rates_sp_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -136,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -148,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_vicon_position_estimate(msg); break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -164,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; @@ -272,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { /* optical flow */ @@ -359,6 +423,45 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fix this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + +void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ @@ -386,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -430,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); - - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f4d0c52d8..91125736c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -58,6 +58,7 @@ #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -109,9 +110,11 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); + void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -145,10 +148,10 @@ private: orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _vicon_position_pub; + orb_advert_t _vision_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..042c46afd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -353,6 +353,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,8 +370,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = &_offboard; break; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 05eae047c..81bbaa658 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -59,6 +59,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/home_position.h> #include <uORB/topics/optical_flow.h> #include <mavlink/mavlink_log.h> @@ -80,6 +81,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms @@ -233,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; + float eph_vision = 0.5f; + float epv_vision = 0.5f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -279,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; + + float corr_vision[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -294,6 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + bool vision_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -312,6 +325,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); + struct vision_position_estimate vision; + memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -323,6 +338,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ @@ -616,6 +632,46 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + + /* check no vision circuit breaker is set */ + if (params.no_vision != CBRK_NO_VISION_KEY) { + /* vehicle vision position */ + orb_check(vision_position_estimate_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + + /* reset position estimate on first vision update */ + if (!vision_valid) { + x_est[0] = vision.x; + x_est[1] = vision.vx; + y_est[0] = vision.y; + y_est[1] = vision.vy; + /* only reset the z estimate if the z weight parameter is not zero */ + if (params.w_z_vision_p > MIN_VALID_W) + { + z_est[0] = vision.z; + z_est[1] = vision.vz; + } + + vision_valid = true; + warnx("VISION estimate valid"); + mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); + } + + /* calculate correction for position */ + corr_vision[0][0] = vision.x - x_est[0]; + corr_vision[1][0] = vision.y - y_est[0]; + corr_vision[2][0] = vision.z - z_est[0]; + + /* calculate correction for velocity */ + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -732,14 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } + /* check for timeout on vision topic */ + if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { + vision_valid = false; + warnx("VISION timeout"); + mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); + } + /* check for sonar measurement timeout */ - if (sonar_valid && t > sonar_time + sonar_timeout) { + if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } @@ -759,10 +822,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use VISION if it's valid and has a valid weight parameter */ + bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; + bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -781,6 +847,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_xy_vision_p = params.w_xy_vision_p; + float w_xy_vision_v = params.w_xy_vision_v; + float w_z_vision_p = params.w_z_vision_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -821,6 +891,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* accelerometer bias correction for VISION (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_vision_xy) { + accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; + accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; + } + + if (use_vision_z) { + accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; + } + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; @@ -863,10 +962,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); } + if (use_vision_z) { + epv = fminf(epv, epv_vision); + inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); + } + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); corr_baro = 0; } else { @@ -904,11 +1009,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_vision_xy) { + eph = fminf(eph, eph_vision); + + inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); + inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); + + if (w_xy_vision_v > MIN_VALID_W) { + inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); + } + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_flow, 0, sizeof(corr_flow)); } else { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 98b31d17b..cc0fb4155 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /* * @file position_estimator_inav_params.c * + * @author Anton Babushkin <rk3dov@gmail.com> + * * Parameters for position_estimator_inav */ @@ -63,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); /** + * Z axis weight for vision + * + * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); + +/** * Z axis weight for sonar * * Weight (cutoff frequency) for sonar measurements. @@ -96,6 +108,28 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); /** + * XY axis weight for vision position + * + * Weight (cutoff frequency) for vision position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); + +/** + * XY axis weight for vision velocity + * + * Weight (cutoff frequency) for vision velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); + +/** * XY axis weight for optical flow * * Weight (cutoff frequency) for optical flow (velocity) measurements. @@ -232,13 +266,27 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); +/** + * Disable vision input + * + * Set to the appropriate key (328754) to disable vision input. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -250,6 +298,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; @@ -259,9 +308,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); + param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); @@ -273,6 +325,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); return OK; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 423f0d879..d0a65e42e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,9 +32,11 @@ ****************************************************************************/ /* - * @file position_estimator_inav_params.h + * @file position_estimator_inav_params.c * - * Parameters for Position Estimator + * @author Anton Babushkin <rk3dov@gmail.com> + * + * Parameters definition for position_estimator_inav */ #include <systemlib/param/param.h> @@ -43,9 +44,12 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; + float w_xy_vision_p; + float w_xy_vision_v; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -57,15 +61,19 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; float delay_gps; }; struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; + param_t w_xy_vision_p; + param_t w_xy_vision_v; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; @@ -77,9 +85,12 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t no_vision; param_t delay_gps; }; +#define CBRK_NO_VISION_KEY 328754 + /** * Initialize all parameter handles and values * diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70..0da778b6f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b49452..dbda8ea6f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/satellite_info.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> @@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435..6741c7e25 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,20 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7ce6ef5ef..229bfe3ce 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensor Calibration + * @unit hPa + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + /** * Board rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4e8a8c01d..cdcb428dd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -143,6 +143,12 @@ #define STICK_ON_OFF_LIMIT 0.75f +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * Sensor app start / stop handling function * @@ -235,7 +241,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -258,7 +264,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +307,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +362,11 @@ private: param_t board_rotation; param_t external_mag_rotation; - + param_t board_offset[3]; + param_t baro_qnh; + } _parameter_handles; /**< handles for interesting parameters */ @@ -462,12 +472,6 @@ private: namespace sensors { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Sensors *g_sensors = nullptr; } @@ -611,12 +615,15 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); - + /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + /* fetch initial parameter values */ parameters_update(); } @@ -828,19 +835,37 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); - + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - + /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; + math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + _board_rotation = _board_rotation * board_rotation_offset; + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int fd; + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + + } else { + int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (ret) { + warnx("qnh could not be set"); + close(fd); + return ERROR; + } + close(fd); + } + return OK; } @@ -1204,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1432,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 08c3ce1ac..f7d5f8737 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -213,6 +213,9 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); +#include "topics/vision_position_estimate.h" +ORB_DEFINE(vision_position_estimate, struct vision_position_estimate); + #include "topics/vehicle_force_setpoint.h" ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index cd48d3cb2..7342fcf04 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,7 +54,6 @@ struct differential_pressure_s { uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ uint64_t error_count; /**< Number of errors detected by driver */ - float differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 7db33d98b..44aa50572 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file vehicle_command.h * Definition of the vehicle command uORB topic. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -52,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -87,7 +91,8 @@ enum VEHICLE_CMD { VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END = 501, /* | */ + VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ + VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ }; /** @@ -115,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h new file mode 100644 index 000000000..74c96cf2e --- /dev/null +++ b/src/modules/uORB/topics/vision_position_estimate.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vision_position_estimate.h + * Vision based position estimate + */ + +#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_ +#define TOPIC_VISION_POSITION_ESTIMATE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Vision based position estimate in NED frame + */ +struct vision_position_estimate { + + unsigned id; /**< ID of the estimator, commonly the component ID of the incoming message */ + + uint64_t timestamp_boot; /**< time of this estimate, in microseconds since system start */ + uint64_t timestamp_computer; /**< timestamp provided by the companion computer, in us */ + + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< Y position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + + float vx; /**< X velocity in meters per second in NED earth-fixed frame */ + float vy; /**< Y velocity in meters per second in NED earth-fixed frame */ + float vz; /**< Z velocity in meters per second in NED earth-fixed frame */ + + float q[4]; /**< Estimated attitude as quaternion */ + + // XXX Add covariances here + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vision_position_estimate); + +#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */ diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp index 406eba88c..223d94731 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file esc_controller.cpp + * @file esc.cpp * * @author Pavel Kirienko <pavel.kirienko@gmail.com> */ -#include "esc_controller.hpp" +#include "esc.hpp" #include <systemlib/err.h> UavcanEscController::UavcanEscController(uavcan::INode &node) : diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp index 559ede561..cf0988210 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file esc_controller.hpp + * @file esc.hpp * * UAVCAN <--> ORB bridge for ESC messages: * uavcan.equipment.esc.RawCommand diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f2468..f92bc754f 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ - gnss_receiver.cpp +# Main +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + uavcan_params.c + +# Actuators +SRCS += actuators/esc.cpp + +# Sensors +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp \ + sensors/mag.cpp \ + sensors/baro.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 000000000..80c5e3828 --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "baro.hpp" +#include <cmath> + +static const orb_id_t BARO_TOPICS[2] = { + ORB_ID(sensor_baro0), + ORB_ID(sensor_baro1) +}; + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +_sub_air_data(node) +{ +} + +int UavcanBarometerBridge::init() +{ + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 000000000..9d470219e --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include <drivers/drv_baro.h> + +#include <uavcan/equipment/air_data/StaticAirData.hpp> + +class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg); + + typedef uavcan::MethodBinder<UavcanBarometerBridge*, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)> + AirDataCbBinder; + + uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data; + unsigned _msl_pressure = 101325; +}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp index ba1fe5e49..0d67aad47 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -32,52 +32,74 @@ ****************************************************************************/ /** - * @file gnss_receiver.cpp + * @file gnss.cpp * * @author Pavel Kirienko <pavel.kirienko@gmail.com> * @author Andrew Chambers <achamber@gmail.com> * */ -#include "gnss_receiver.hpp" +#include "gnss.hpp" #include <systemlib/err.h> #include <mathlib/mathlib.h> #define MM_PER_CM 10 // Millimeters per centimeter -UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : +const char *const UavcanGnssBridge::NAME = "gnss"; + +UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), -_uavcan_sub_status(node), +_sub_fix(node), _report_pub(-1) { } -int UavcanGnssReceiver::init() +int UavcanGnssBridge::init() { - int res = -1; - - // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; +} - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} - return res; +void UavcanGnssBridge::print_status() const +{ + printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { + printf("N/A\n"); + } else { + printf("%d\n", _receiver_node_id); + } } -void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = msg.lat_1e7; - _report.lon = msg.lon_1e7; - _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + warnx("GNSS receiver node ID: %d", _receiver_node_id); + } else { + if (_receiver_node_id != msg.getSrcNodeID().get()) { + return; // This GNSS receiver is the redundant one, ignore it. + } + } + + auto report = ::vehicle_gps_position_s(); + + report.timestamp_position = hrt_absolute_time(); + report.lat = msg.lat_1e7; + report.lon = msg.lon_1e7; + report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) - _report.timestamp_variance = _report.timestamp_position; + report.timestamp_variance = report.timestamp_position; // Check if the msg contains valid covariance information @@ -90,19 +112,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav // Horizontal position uncertainty const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); - _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.eph = -1.0F; - _report.epv = -1.0F; + report.eph = -1.0F; + report.epv = -1.0F; } if (valid_velocity_covariance) { float vel_cov[9]; msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance @@ -118,36 +140,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav float vel_e = msg.ned_velocity[1]; float vel_n_sq = vel_n * vel_n; float vel_e_sq = vel_e * vel_e; - _report.c_variance_rad = + report.c_variance_rad = (vel_e_sq * vel_cov[0] + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); } else { - _report.s_variance_m_s = -1.0F; - _report.c_variance_rad = -1.0F; + report.s_variance_m_s = -1.0F; + report.c_variance_rad = -1.0F; } - _report.fix_type = msg.status; + report.fix_type = msg.status; - _report.timestamp_velocity = _report.timestamp_position; - _report.vel_n_m_s = msg.ned_velocity[0]; - _report.vel_e_m_s = msg.ned_velocity[1]; - _report.vel_d_m_s = msg.ned_velocity[2]; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); - _report.vel_ned_valid = true; + report.timestamp_velocity = report.timestamp_position; + report.vel_n_m_s = msg.ned_velocity[0]; + report.vel_e_m_s = msg.ned_velocity[1]; + report.vel_d_m_s = msg.ned_velocity[2]; + report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s); + report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s); + report.vel_ned_valid = true; - _report.timestamp_time = _report.timestamp_position; - _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + report.timestamp_time = report.timestamp_position; + report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds - _report.satellites_used = msg.sats_used; + report.satellites_used = msg.sats_used; if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report); } } diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp index 18df8da2f..e8466b401 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gnss_receiver.hpp + * @file gnss.hpp * * UAVCAN --> ORB bridge for GNSS messages: * uavcan.equipment.gnss.Fix @@ -51,12 +51,22 @@ #include <uavcan/uavcan.hpp> #include <uavcan/equipment/gnss/Fix.hpp> -class UavcanGnssReceiver +#include "sensor_bridge.hpp" + +class UavcanGnssBridge : public IUavcanSensorBridge { public: - UavcanGnssReceiver(uavcan::INode& node); + static const char *const NAME; + + UavcanGnssBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; - int init(); + unsigned get_num_redundant_channels() const override; + + void print_status() const override; private: /** @@ -64,21 +74,14 @@ private: */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg); - - typedef uavcan::MethodBinder<UavcanGnssReceiver*, - void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)> + typedef uavcan::MethodBinder<UavcanGnssBridge*, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)> FixCbBinder; - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status; + uavcan::INode &_node; + uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix; + int _receiver_node_id = -1; - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 000000000..8e6a9a22f --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "mag.hpp" + +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + +const char *const UavcanMagnetometerBridge::NAME = "mag"; + +UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +_sub_mag(node) +{ + _scale.x_scale = 1.0F; + _scale.y_scale = 1.0F; + _scale.z_scale = 1.0F; +} + +int UavcanMagnetometerBridge::init() +{ + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; +} + +int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case MAGIOCSSCALE: { + std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { + std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { + return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + } + case MAGIOCSSAMPLERATE: { + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCEXSTRAP: + case MAGIOCGLOWPASS: { + return -EINVAL; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg) +{ + auto report = ::mag_report(); + + report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 000000000..6d413a8f7 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include <drivers/drv_mag.h> + +#include <uavcan/equipment/ahrs/Magnetometer.hpp> + +class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanMagnetometerBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg); + + typedef uavcan::MethodBinder<UavcanMagnetometerBridge*, + void (UavcanMagnetometerBridge::*) + (const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)> + MagCbBinder; + + uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag; + mag_scale _scale = {}; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 000000000..9608ce680 --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "sensor_bridge.hpp" +#include <cassert> + +#include "gnss.hpp" +#include "mag.hpp" +#include "baro.hpp" + +/* + * IUavcanSensorBridge + */ +void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list) +{ + list.add(new UavcanBarometerBridge(node)); + list.add(new UavcanMagnetometerBridge(node)); + list.add(new UavcanGnssBridge(node)); +} + +/* + * UavcanCDevSensorBridgeBase + */ +UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() +{ + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + (void)unregister_class_devname(_class_devname, _channels[i].class_instance); + } + } + delete [] _orb_topics; + delete [] _channels; +} + +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) +{ + assert(report != nullptr); + + Channel *channel = nullptr; + + // Checking if such channel already exists + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id == node_id) { + channel = _channels + i; + break; + } + } + + // No such channel - try to create one + if (channel == nullptr) { + if (_out_of_channels) { + return; // Give up immediately - saves some CPU time + } + + log("adding channel %d...", node_id); + + // Search for the first free channel + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id < 0) { + channel = _channels + i; + break; + } + } + + // No free channels left + if (channel == nullptr) { + _out_of_channels = true; + log("out of channels"); + return; + } + + // Ask the CDev helper which class instance we can take + const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { + _out_of_channels = true; + log("out of class instances"); + (void)unregister_class_devname(_class_devname, class_instance); + return; + } + + // Publish to the appropriate topic, abort on failure + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; + + channel->orb_advert = orb_advertise(channel->orb_id, report); + if (channel->orb_advert < 0) { + log("ADVERTISE FAILED"); + (void)unregister_class_devname(_class_devname, class_instance); + *channel = Channel(); + return; + } + + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); + } + assert(channel != nullptr); + + (void)orb_publish(channel->orb_id, channel->orb_advert, report); +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +{ + unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + out += 1; + } + } + return out; +} + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + printf("channel %d: node id %d --> class instance %d\n", + i, _channels[i].node_id, _channels[i].class_instance); + } else { + printf("channel %d: empty\n", i); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 000000000..1316f7ecc --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <containers/List.hpp> +#include <uavcan/uavcan.hpp> +#include <drivers/device/device.h> +#include <drivers/drv_orb_dev.h> + +/** + * A sensor bridge class must implement this interface. + */ +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*> +{ +public: + static constexpr unsigned MAX_NAME_LEN = 20; + + virtual ~IUavcanSensorBridge() { } + + /** + * Returns ASCII name of the bridge. + */ + virtual const char *get_name() const = 0; + + /** + * Starts the bridge. + * @return Non-negative value on success, negative on error. + */ + virtual int init() = 0; + + /** + * Returns number of active redundancy channels. + */ + virtual unsigned get_num_redundant_channels() const = 0; + + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + + /** + * Sensor bridge factory. + * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". + * @return nullptr if such bridge can't be created. + */ + static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list); +}; + +/** + * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. + * For example, sensor_mag0, sensor_mag1, etc. + */ +class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +{ + struct Channel + { + int node_id = -1; + orb_id_t orb_id = nullptr; + orb_advert_t orb_advert = -1; + int class_instance = -1; + }; + + const unsigned _max_channels; + const char *const _class_devname; + orb_id_t *const _orb_topics; + Channel *const _channels; + bool _out_of_channels = false; + +protected: + template <unsigned MaxChannels> + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, + const orb_id_t (&orb_topics)[MaxChannels]) : + device::CDev(name, devname), + _max_channels(MaxChannels), + _class_devname(class_devname), + _orb_topics(new orb_id_t[MaxChannels]), + _channels(new Channel[MaxChannels]) + { + memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + } + + /** + * Sends one measurement into appropriate ORB topic. + * New redundancy channels will be registered automatically. + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object + */ + void publish(const int node_id, const void *report); + +public: + virtual ~UavcanCDevSensorBridgeBase(); + + unsigned get_num_redundant_channels() const override; + + void print_status() const override; +}; diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index e41d5f953..fe8ba406a 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment) (void)adjustment; } +uavcan::uint64_t getUtcUSecFromCanInterrupt(); + uavcan::uint64_t getUtcUSecFromCanInterrupt() { return 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6a..a8485ee44 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -38,8 +38,10 @@ #include <fcntl.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <systemlib/mixer/mixer.h> #include <systemlib/board_serial.h> +#include <systemlib/scheduling_priorities.h> #include <version/version.h> #include <arch/board/board.h> #include <arch/chip/chip.h> @@ -65,16 +67,18 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node), - _gnss_receiver(_node) + _node_mutex(), + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - // memset(_controls, 0, sizeof(_controls)); - // memset(_poll_fds, 0, sizeof(_poll_fds)); + const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { + std::abort(); + } } UavcanNode::~UavcanNode() @@ -99,10 +103,18 @@ UavcanNode::~UavcanNode() } /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); ::close(_armed_sub); + // Removing the sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + auto next = br->getSibling(); + delete br; + br = next; + } + _instance = nullptr; } @@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast<main_t>(run_trampoline), nullptr); if (_instance->_task < 0) { @@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; - /* do regular cdev init */ + // Do regular cdev init ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } _node.setName("org.pixhawk.pixhawk"); @@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id) fill_node_info(); - /* initializing the bridges UAVCAN <--> uORB */ + // Actuators ret = _esc_controller.init(); - if (ret < 0) + if (ret < 0) { return ret; + } - ret = _gnss_receiver.init(); - if (ret < 0) - return ret; + // Sensor bridges + IUavcanSensorBridge::make_all(_node, _sensor_bridges); + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + ret = br->init(); + if (ret < 0) { + warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + return ret; + } + warnx("sensor bridge '%s' init ok", br->get_name()); + br = br->getSibling(); + } return _node.start(); } @@ -248,6 +271,8 @@ void UavcanNode::node_spin_once() int UavcanNode::run() { + (void)pthread_mutex_lock(&_node_mutex); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count @@ -291,8 +316,13 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } + // Mutex is unlocked while the thread is blocked on IO multiplexing + (void)pthread_mutex_unlock(&_node_mutex); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + (void)pthread_mutex_lock(&_node_mutex); + node_spin_once(); // Non-blocking // this would be bad... @@ -352,7 +382,6 @@ int UavcanNode::run() // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } - } // Check arming state @@ -376,10 +405,7 @@ int UavcanNode::run() } int -UavcanNode::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -520,8 +546,23 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + (void)pthread_mutex_lock(&_node_mutex); + + // ESC mixer status + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + + // Sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); + br = br->getSibling(); + } + + (void)pthread_mutex_unlock(&_node_mutex); } /* @@ -529,79 +570,57 @@ UavcanNode::print_info() */ static void print_usage() { - warnx("usage: uavcan start <node_id> [can_bitrate]"); + warnx("usage: \n" + "\tuavcan {start|status|stop}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; - if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - if (argc < 3) { - print_usage(); - ::exit(1); + if (UavcanNode::instance()) { + errx(1, "already started"); } - /* - * Node ID - */ - const int node_id = atoi(argv[2]); + // Node ID + int32_t node_id = 0; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } - /* - * CAN bitrate - */ - unsigned bitrate = 0; - - if (argc > 3) { - bitrate = atol(argv[3]); - } - - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - if (UavcanNode::instance()) { - errx(1, "already started"); - } - - /* - * Start - */ + // Start warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } /* commands below require the app to be started */ - UavcanNode *inst = UavcanNode::instance(); + UavcanNode *const inst = UavcanNode::instance(); if (!inst) { errx(1, "application not running"); } if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - - inst->print_info(); - return OK; + inst->print_info(); + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - - delete inst; - inst = nullptr; - return OK; + delete inst; + ::exit(0); } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b..be7db9741 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,8 +42,8 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> -#include "esc_controller.hpp" -#include "gnss_receiver.hpp" +#include "actuators/esc.hpp" +#include "sensors/sensor_bridge.hpp" /** * @file uavcan_main.hpp @@ -77,12 +77,10 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node& get_node() { return _node; } - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + // TODO: move the actuator mixing stuff into the ESC controller class + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); void subscribe(); @@ -109,8 +107,11 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance + pthread_mutex_t _node_mutex; + UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + + List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 000000000..e6ea8a8fb --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Enable UAVCAN. + * + * Enables support for UAVCAN-interfaced actuators and sensors. + * + * @min 0 + * @max 1 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); + +/** + * UAVCAN Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1); + +/** + * UAVCAN CAN bus bitrate. + * + * @min 20000 + * @max 1000000 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); + + + diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk index f00b0f592..5000790a5 100644 --- a/src/modules/unit_test/module.mk +++ b/src/modules/unit_test/module.mk @@ -37,3 +37,4 @@ SRCS = unit_test.cpp +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk index 990c56768..ce87eb3e2 100644 --- a/src/systemcmds/esc_calib/module.mk +++ b/src/systemcmds/esc_calib/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = esc_calib SRCS = esc_calib.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index b3fceceb5..1bc4f414e 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index e2fa0ff80..7d2c59f91 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c MODULE_STACKSIZE = 1400 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk index 13a24150f..a51ac8e0c 100644 --- a/src/systemcmds/pwm/module.mk +++ b/src/systemcmds/pwm/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = pwm SRCS = pwm.c MODULE_STACKSIZE = 1800 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index a4f17eebd..e005bf9c1 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -331,7 +331,7 @@ mag(int argc, char *argv[]) float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); - if (len < 1.0f || len > 3.0f) { + if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } |