diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index 142da2d581..8d16e538a0 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) # -# Too high RAM usage due to static allocations +# Removed from build due to large static allocations #MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index fdae381aba..2eca557465 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -1,8 +1,12 @@ uint64 timestamp +uint64 integral_dt # integration time uint64 error_count float32 x # acceleration in the NED X board axis in m/s^2 float32 y # acceleration in the NED Y board axis in m/s^2 float32 z # acceleration in the NED Z board axis in m/s^2 +float32 x_integral # velocity in the NED X board axis in m/s over the integration time frame +float32 y_integral # velocity in the NED Y board axis in m/s over the integration time frame +float32 z_integral # velocity in the NED Z board axis in m/s over the integration time frame float32 temperature # temperature in degrees celsius float32 range_m_s2 # range in m/s^2 (+- this value) float32 scaling diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 876ccd7c4a..ec58e6f923 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -26,96 +26,49 @@ uint32 SENSOR_PRIO_MAX = 255 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only uint64 timestamp # Timestamp in microseconds since boot, from gyro -# -int16[3] gyro_raw # Raw sensor values of angular velocity -float32[3] gyro_rad_s # Angular velocity in radian per seconds -uint32 gyro_priority # Sensor priority -uint32 gyro_errcount # Error counter for gyro 0 -float32 gyro_temp # Temperature of gyro 0 +uint64[3] gyro_timestamp # Gyro timestamps +int16[9] gyro_raw # Raw sensor values of angular velocity +float32[9] gyro_rad_s # Angular velocity in radian per seconds +uint32[3] gyro_priority # Sensor priority +float32[9] gyro_integral_rad # delta angle in radians +uint64[3] gyro_integral_dt # delta time for gyro integral in us +uint32[3] gyro_errcount # Error counter for gyro 0 +float32[3] gyro_temp # Temperature of gyro 0 -int16[3] accelerometer_raw # Raw acceleration in NED body frame -float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 -int16 accelerometer_mode # Accelerometer measurement mode -float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 -uint64 accelerometer_timestamp # Accelerometer timestamp -uint32 accelerometer_priority # Sensor priority -uint32 accelerometer_errcount # Error counter for accel 0 -float32 accelerometer_temp # Temperature of accel 0 +int16[9] accelerometer_raw # Raw acceleration in NED body frame +float32[9] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +float32[9] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 +uint64[3] accelerometer_integral_dt # delta time for accel integral in us +int16[3] accelerometer_mode # Accelerometer measurement mode +float32[3] accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64[3] accelerometer_timestamp # Accelerometer timestamp +uint32[3] accelerometer_priority # Sensor priority +uint32[3] accelerometer_errcount # Error counter for accel 0 +float32[3] accelerometer_temp # Temperature of accel 0 -int16[3] magnetometer_raw # Raw magnetic field in NED body frame -float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss -int16 magnetometer_mode # Magnetometer measurement mode -float32 magnetometer_range_ga # measurement range in Gauss -float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor -uint64 magnetometer_timestamp # Magnetometer timestamp -uint32 magnetometer_priority # Sensor priority -uint32 magnetometer_errcount # Error counter for mag 0 -float32 magnetometer_temp # Temperature of mag 0 +int16[9] magnetometer_raw # Raw magnetic field in NED body frame +float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16[3] magnetometer_mode # Magnetometer measurement mode +float32[3] magnetometer_range_ga # measurement range in Gauss +float32[3] magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64[3] magnetometer_timestamp # Magnetometer timestamp +uint32[3] magnetometer_priority # Sensor priority +uint32[3] magnetometer_errcount # Error counter for mag 0 +float32[3] magnetometer_temp # Temperature of mag 0 -int16[3] gyro1_raw # Raw sensor values of angular velocity -float32[3] gyro1_rad_s # Angular velocity in radian per seconds -uint64 gyro1_timestamp # Gyro timestamp -uint32 gyro1_priority # Sensor priority -uint32 gyro1_errcount # Error counter for gyro 1 -float32 gyro1_temp # Temperature of gyro 1 - -int16[3] accelerometer1_raw # Raw acceleration in NED body frame -float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer1_timestamp # Accelerometer timestamp -uint32 accelerometer1_priority # Sensor priority -uint32 accelerometer1_errcount # Error counter for accel 1 -float32 accelerometer1_temp # Temperature of accel 1 - -int16[3] magnetometer1_raw # Raw magnetic field in NED body frame -float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer1_timestamp # Magnetometer timestamp -uint32 magnetometer1_priority # Sensor priority -uint32 magnetometer1_errcount # Error counter for mag 1 -float32 magnetometer1_temp # Temperature of mag 1 - -int16[3] gyro2_raw # Raw sensor values of angular velocity -float32[3] gyro2_rad_s # Angular velocity in radian per seconds -uint64 gyro2_timestamp # Gyro timestamp -uint32 gyro2_priority # Sensor priority -uint32 gyro2_errcount # Error counter for gyro 1 -float32 gyro2_temp # Temperature of gyro 1 - -int16[3] accelerometer2_raw # Raw acceleration in NED body frame -float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 -uint64 accelerometer2_timestamp # Accelerometer timestamp -uint32 accelerometer2_priority # Sensor priority -uint32 accelerometer2_errcount # Error counter for accel 2 -float32 accelerometer2_temp # Temperature of accel 2 - -int16[3] magnetometer2_raw # Raw magnetic field in NED body frame -float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss -uint64 magnetometer2_timestamp # Magnetometer timestamp -uint32 magnetometer2_priority # Sensor priority -uint32 magnetometer2_errcount # Error counter for mag 2 -float32 magnetometer2_temp # Temperature of mag 2 - -float32 baro_pres_mbar # Barometric pressure, already temp. comp. -float32 baro_alt_meter # Altitude, already temp. comp. -float32 baro_temp_celcius # Temperature in degrees celsius -uint64 baro_timestamp # Barometer timestamp -uint32 baro_priority # Sensor priority - -float32 baro1_pres_mbar # Barometric pressure, already temp. comp. -float32 baro1_alt_meter # Altitude, already temp. comp. -float32 baro1_temp_celcius # Temperature in degrees celsius -uint64 baro1_timestamp # Barometer timestamp -uint32 baro1_priority # Sensor priority +float32[3] baro_pres_mbar # Barometric pressure, already temp. comp. +float32[3] baro_alt_meter # Altitude, already temp. comp. +float32[3] baro_temp_celcius # Temperature in degrees celsius +uint64[3] baro_timestamp # Barometer timestamp +uint32[3] baro_priority # Sensor priority +uint32[3] baro_errcount # Error count in communication float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 uint16[10] adc_mapping # Channel indices of each of these values float32 mcu_temp_celcius # Internal temperature measurement of MCU -float32 differential_pressure_pa # Airspeed sensor differential pressure -uint64 differential_pressure_timestamp # Last measurement timestamp -float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32 differential_pressure_priority # Sensor priority - -float32 differential_pressure1_pa # Airspeed sensor differential pressure -uint64 differential_pressure1_timestamp # Last measurement timestamp -float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading -uint32 differential_pressure1_priority # Sensor priority +float32[3] differential_pressure_pa # Airspeed sensor differential pressure +uint64[3] differential_pressure_timestamp # Last measurement timestamp +float32[3] differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading +uint32[3] differential_pressure_priority # Sensor priority +uint32[3] differential_pressure_errcount # Error count in communication diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index 4a252269e8..65aa4a8706 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -1,8 +1,12 @@ uint64 timestamp +uint64 integral_dt # integration time uint64 error_count float32 x # angular velocity in the NED X board axis in rad/s float32 y # angular velocity in the NED Y board axis in rad/s float32 z # angular velocity in the NED Z board axis in rad/s +float32 x_integral # delta angle in the NED X board axis in rad/s in the integration time frame +float32 y_integral # delta angle in the NED Y board axis in rad/s in the integration time frame +float32 z_integral # delta angle in the NED Z board axis in rad/s in the integration time frame float32 temperature # temperature in degrees celcius float32 range_rad_s float32 scaling diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 63879149ad..0ee90cd61d 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -10,6 +10,9 @@ float32 yawspeed # Yaw body angular rate (rad/s, x forward/y right/z down) float32 rollacc # Roll angular accelration (rad/s^2, x forward/y right/z down) float32 pitchacc # Pitch angular acceleration (rad/s^2, x forward/y right/z down) float32 yawacc # Yaw angular acceleration (rad/s^2, x forward/y right/z down) +float32 rate_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 accel_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. +float32 mag_vibration # Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels. float32[3] rate_offsets # Offsets of the body angular rates from zero float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) float32[4] q # Quaternion (NED) diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h new file mode 100644 index 0000000000..6dc886de96 --- /dev/null +++ b/src/drivers/device/integrator.h @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 integrator.h + * + * A resettable integrator + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +#include + +class Integrator { +public: + Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); + virtual ~Integrator(); + + /** + * Put an item into the integral. + * + * @param timestamp Timestamp of the current value + * @param val Item to put + * @param integral Current integral in case the integrator did reset, else the value will not be modified + * @return true if putting the item triggered an integral reset + * and the integral should be published + */ + bool put(hrt_abstime timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt); + + /** + * Get the current integral value + * + * @return the integral since the last auto-reset + */ + math::Vector<3> get() { return _integral_auto; } + + /** + * Read from the integral + * + * @param auto_reset Reset the integral to zero on read + * @return the integral since the last read-reset + */ + math::Vector<3> read(bool auto_reset); + + /** + * Get current integral start time + */ + hrt_abstime current_integral_start() { return _last_auto; } + +private: + hrt_abstime _auto_reset_interval; /**< the interval after which the content will be published and the integrator reset */ + hrt_abstime _last_integration; /**< timestamp of the last integration step */ + hrt_abstime _last_auto; /**< last auto-announcement of integral value */ + math::Vector<3> _integral_auto; /**< the integrated value which auto-resets after _auto_reset_interval */ + math::Vector<3> _integral_read; /**< the integrated value since the last read */ + math::Vector<3> _last_val; /**< previously integrated last value */ + math::Vector<3> _last_delta; /**< last local delta */ + void (*_auto_callback)(hrt_abstime, math::Vector<3>); /**< the function callback for auto-reset */ + bool _coning_comp_on; /**< coning compensation */ + + /* we don't want this class to be copied */ + Integrator(const Integrator&); + Integrator operator=(const Integrator&); +}; + +Integrator::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) : + _auto_reset_interval(auto_reset_interval), + _last_integration(0), + _last_auto(0), + _integral_auto(0.0f, 0.0f, 0.0f), + _integral_read(0.0f, 0.0f, 0.0f), + _last_val(0.0f, 0.0f, 0.0f), + _last_delta(0.0f, 0.0f, 0.0f), + _auto_callback(nullptr), + _coning_comp_on(coning_compensation) +{ + +} + +Integrator::~Integrator() +{ + +} + +bool +Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt) +{ + bool auto_reset = false; + + if (_last_integration == 0) { + /* this is the first item in the integrator */ + _last_integration = timestamp; + _last_auto = timestamp; + _last_val = val; + return false; + } + + // Integrate + double dt = (double)(timestamp - _last_integration) / 1000000.0; + math::Vector<3> i = (val + _last_val) * dt * 0.5f; + + // Apply coning compensation if required + if (_coning_comp_on) { + // Coning compensation derived by Paul Riseborough and Jonathan Challinger, + // following: + // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation + // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf + + i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f; + } + + _integral_auto += i; + _integral_read += i; + + _last_integration = timestamp; + _last_val = val; + _last_delta = i; + + if ((timestamp - _last_auto) > _auto_reset_interval) { + if (_auto_callback) { + /* call the callback */ + _auto_callback(timestamp, _integral_auto); + } + + integral = _integral_auto; + integral_dt = (timestamp - _last_auto); + + auto_reset = true; + _last_auto = timestamp; + _integral_auto(0) = 0.0f; + _integral_auto(1) = 0.0f; + _integral_auto(2) = 0.0f; + } + + return auto_reset; +} + +math::Vector<3> +Integrator::read(bool auto_reset) +{ + math::Vector<3> val = _integral_read; + if (auto_reset) { + _integral_read(0) = 0.0f; + _integral_read(1) = 0.0f; + _integral_read(2) = 0.0f; + } + + return val; +} diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index de22a888d3..d8650c382e 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -178,12 +178,12 @@ void frsky_send_frame1(int uart) roundf(raw.accelerometer_m_s2[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - raw.baro_alt_meter); + raw.baro_alt_meter[0]); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(raw.baro_alt_meter) * 100.0f)); + roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(raw.baro_temp_celcius)); + roundf(raw.baro_temp_celcius[0])); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(battery.voltage_v * 10.0f)); diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index ef841c3f85..61daa302fb 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -148,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7dcd5a0004..43a24359eb 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -175,6 +176,7 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_RATE 760 #define L3G4200D_DEFAULT_RATE 800 +#define L3GD20_MAX_OUTPUT_RATE 280 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 @@ -256,6 +258,8 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _gyro_int; + /* true if an L3G4200D is detected */ bool _is_l3g4200d; @@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true), _is_l3g4200d(false), _rotation(rotation), _checked_next(0) @@ -1029,13 +1034,21 @@ L3GD20::measure() // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report.x = _gyro_filter_x.apply(report.x); - report.y = _gyro_filter_y.apply(report.y); - report.z = _gyro_filter_z.apply(report.z); + report.x = _gyro_filter_x.apply(xin); + report.y = _gyro_filter_y.apply(yin); + report.z = _gyro_filter_z.apply(zin); + + math::Vector<3> gval(xin, yin, zin); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt); + report.x_integral = gval_integrated(0); + report.y_integral = gval_integrated(1); + report.z_integral = gval_integrated(2); report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; @@ -1044,13 +1057,15 @@ L3GD20::measure() _reports->force(&report); - /* notify anyone waiting for data */ - poll_notify(POLLIN); + if (gyro_notify) { + /* notify anyone waiting for data */ + poll_notify(POLLIN); - /* publish for subscribers */ - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + /* publish for subscribers */ + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + } } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 52e514a3e1..4235ecc6f5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include @@ -206,6 +207,7 @@ static const int ERROR = -1; #define LSM303D_ACCEL_DEFAULT_RATE 800 #define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 #define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define LSM303D_ACCEL_MAX_OUTPUT_RATE 280 #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 @@ -308,6 +310,8 @@ private: math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; + Integrator _accel_int; + enum Rotation _rotation; // values used to @@ -577,6 +581,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true), _rotation(rotation), _constant_accel_count(0), _last_temperature(0), @@ -1411,6 +1416,13 @@ LSM303D::stop() { hrt_cancel(&_accel_call); hrt_cancel(&_mag_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _mag_reports->flush(); } void @@ -1575,17 +1587,27 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); + accel_report.x_integral = aval_integrated(0); + accel_report.y_integral = aval_integrated(1); + accel_report.z_integral = aval_integrated(2); + accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; _accel_reports->force(&accel_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + if (accel_notify) { + poll_notify(POLLIN); - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } } _accel_read++; @@ -1841,7 +1863,7 @@ namespace lsm303d LSM303D *g_dev; -void start(bool external_bus, enum Rotation rotation); +void start(bool external_bus, enum Rotation rotation, unsigned range); void test(); void reset(); void info(); @@ -1856,11 +1878,12 @@ void test_error(); * up and running or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation) +start(bool external_bus, enum Rotation rotation, unsigned range) { int fd, fd_mag; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already started"); + } /* create the driver */ if (external_bus) { @@ -1884,11 +1907,17 @@ start(bool external_bus, enum Rotation rotation) /* set the poll rate to default, starts automatic data collection */ fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } + + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { + goto fail; + } fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); @@ -1980,7 +2009,10 @@ test() warnx("mag z: \t%d\traw", (int)m_report.z_raw); warnx("mag range: %8.4f ga", (double)m_report.range_ga); - /* XXX add poll-rate tests here too */ + /* reset to default polling */ + if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "reset to default polling"); + } close(fd_accel); close(fd_mag); @@ -2084,9 +2116,10 @@ lsm303d_main(int argc, char *argv[]) bool external_bus = false; int ch; enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:")) != EOF) { + while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; @@ -2094,6 +2127,9 @@ lsm303d_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': + accel_range = atoi(optarg); + break; default: lsm303d::usage(); exit(0); @@ -2107,7 +2143,7 @@ lsm303d_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) - lsm303d::start(external_bus, rotation); + lsm303d::start(external_bus, rotation, accel_range); /* * Test the driver/device. diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7c7bc43d3e..4af1bec586 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -69,6 +69,7 @@ #include #include +#include #include #include #include @@ -167,10 +168,13 @@ #define MPU6000_ACCEL_DEFAULT_RANGE_G 8 #define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280 #define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU6000_GYRO_DEFAULT_RANGE_G 8 #define MPU6000_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE #define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 @@ -281,6 +285,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + Integrator _accel_int; + Integrator _gyro_int; + enum Rotation _rotation; // this is used to support runtime checking of key @@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_next(0), _in_factory_test(false), @@ -1525,6 +1534,13 @@ void MPU6000::stop() { hrt_cancel(&_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); } void @@ -1759,6 +1775,14 @@ MPU6000::measure() arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1786,6 +1810,14 @@ MPU6000::measure() grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; @@ -1796,10 +1828,15 @@ MPU6000::measure() _gyro_reports->force(&grb); /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + if (accel_notify) { + poll_notify(POLLIN); + } - if (!(_pub_blocked)) { + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); @@ -1807,7 +1844,7 @@ MPU6000::measure() orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (!(_pub_blocked)) { + if (gyro_notify && !(_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1925,7 +1962,7 @@ namespace mpu6000 MPU6000 *g_dev_int; // on internal bus MPU6000 *g_dev_ext; // on external bus -void start(bool, enum Rotation); +void start(bool, enum Rotation, int range); void stop(bool); void test(bool); void reset(bool); @@ -1942,7 +1979,7 @@ void usage(); * or failed to detect the sensor. */ void -start(bool external_bus, enum Rotation rotation) +start(bool external_bus, enum Rotation rotation, int range) { int fd; MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; @@ -1979,6 +2016,9 @@ start(bool external_bus, enum Rotation rotation) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) + goto fail; + close(fd); exit(0); @@ -2076,6 +2116,12 @@ test(bool external_bus) warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); + /* reset to default polling */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "reset to default polling"); + + close(fd); + close(fd_gyro); /* XXX add poll-rate tests here too */ @@ -2175,6 +2221,7 @@ usage() warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); + warnx(" -a accel range (in g)"); } } // namespace @@ -2185,9 +2232,10 @@ mpu6000_main(int argc, char *argv[]) bool external_bus = false; int ch; enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XR:")) != EOF) { + while ((ch = getopt(argc, argv, "XR:a:")) != EOF) { switch (ch) { case 'X': external_bus = true; @@ -2195,6 +2243,9 @@ mpu6000_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': + accel_range = atoi(optarg); + break; default: mpu6000::usage(); exit(0); @@ -2208,7 +2259,7 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation); + mpu6000::start(external_bus, rotation, accel_range); } if (!strcmp(verb, "stop")) { diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 847d3e3a03..7c336dfcab 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -274,7 +274,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); if (_reports == nullptr) { DEVICE_DEBUG("can't get memory for reports"); diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index aa3c5000a4..39e1f50809 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,6 +38,9 @@ */ #include +#include #define ecl_absolute_time hrt_absolute_time #define ecl_elapsed_time hrt_elapsed_time +#define ECL_WARN PX4_WARN +#define ECL_INFO PX4_INFO diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 03974c9503..747a7c5be0 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \ attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ - l1/ecl_l1_pos_controller.cpp + l1/ecl_l1_pos_controller.cpp \ + validation/data_validator.cpp \ + validation/data_validator_group.cpp MAXOPTIMIZATION = -Os diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp new file mode 100644 index 0000000000..037cb88645 --- /dev/null +++ b/src/lib/ecl/validation/data_validator.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 data_validator.c + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator.h" +#include + +DataValidator::DataValidator(DataValidator *prev_sibling) : + _time_last(0), + _timeout_interval(70000), + _event_count(0), + _error_count(0), + _priority(0), + _mean{0.0f}, + _lp{0.0f}, + _M2{0.0f}, + _rms{0.0f}, + _value{0.0f}, + _value_equal_count(0), + _sibling(prev_sibling) +{ + +} + +DataValidator::~DataValidator() +{ + +} + +void +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) +{ + _event_count++; + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < _dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float +DataValidator::confidence(uint64_t timestamp) +{ + /* check if we have any data */ + if (_time_last == 0) { + return 0.0f; + } + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + return 0.0f; + } + + /* we got the exact same sensor value N times in a row */ + if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + return 0.0f; + } + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + return 0.0f; + } + + return 1.0f; +} + +int +DataValidator::priority() +{ + return _priority; +} + +void +DataValidator::print() +{ + if (_time_last == 0) { + ECL_INFO("\tno data\n"); + return; + } + + for (unsigned i = 0; i < _dimensions; i++) { + ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n", + (double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]); + } +} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h new file mode 100644 index 0000000000..686a810c4e --- /dev/null +++ b/src/lib/ecl/validation/data_validator.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 data_validator.h + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +class __EXPORT DataValidator { +public: + DataValidator(DataValidator *prev_sibling = nullptr); + virtual ~DataValidator(); + + /** + * Put an item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); + + /** + * Get the next sibling in the group + * + * @return the next sibling + */ + DataValidator* sibling() { return _sibling; } + + /** + * Get the confidence of this validator + * @return the confidence between 0 and 1 + */ + float confidence(uint64_t timestamp); + + /** + * Get the error count of this validator + * @return the error count + */ + uint64_t error_count() { return _error_count; } + + /** + * Get the values of this validator + * @return the stored value + */ + float* value() { return _value; } + + /** + * Get the priority of this validator + * @return the stored priority + */ + int priority(); + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float* rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + +private: + static const unsigned _dimensions = 3; + uint64_t _time_last; /**< last timestamp */ + uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ + uint64_t _event_count; /**< total data counter */ + uint64_t _error_count; /**< error count */ + int _priority; /**< sensor nominal priority */ + float _mean[_dimensions]; /**< mean of value */ + float _lp[3]; /**< low pass value */ + float _M2[3]; /**< RMS component value */ + float _rms[3]; /**< root mean square error */ + float _value[3]; /**< last value */ + float _value_equal_count; /**< equal values in a row */ + DataValidator *_sibling; /**< sibling in the group */ + const unsigned NORETURN_ERRCOUNT = 100; /**< if the error count reaches this value, return sensor as invalid */ + const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator&); + DataValidator operator=(const DataValidator&); +}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp new file mode 100644 index 0000000000..cdd1f6a677 --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 data_validator_group.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "data_validator_group.h" +#include + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) : + _first(nullptr), + _curr_best(-1), + _prev_best(-1), + _first_failover_time(0), + _toggle_count(0) +{ + DataValidator *next = _first; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(next); + } + + _first = next; +} + +DataValidatorGroup::~DataValidatorGroup() +{ + +} + +void +DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) +{ + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } +} + +void +DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count, priority); + break; + } + next = next->sibling(); + i++; + } +} + +float* +DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float max_confidence = -1.0f; + int max_priority = -1000; + int max_index = -1; + uint64_t min_error_count = 30000; + DataValidator *best = nullptr; + + unsigned i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + if (confidence > max_confidence || + (fabsf(confidence - max_confidence) < 0.01f && + ((next->error_count() < min_error_count) && + (next->priority() >= max_priority)))) { + max_index = i; + max_confidence = confidence; + max_priority = next->priority(); + min_error_count = next->error_count(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor */ + if (max_index != _curr_best) { + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + *index = max_index; + return (best) ? best->value() : nullptr; +} + +float +DataValidatorGroup::get_vibration_factor(uint64_t timestamp) +{ + DataValidator *next = _first; + + float vibe = 0.0f; + + /* find the best RMS value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* rms = next->rms(); + + for (unsigned j = 0; j < 3; j++) { + if (rms[j] > vibe) { + vibe = rms[j]; + } + } + } + + next = next->sibling(); + } + + return vibe; +} + +void +DataValidatorGroup::print() +{ + /* print the group's state */ + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); + + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + ECL_INFO("sensor #%u:\n", i); + next->print(); + next = next->sibling(); + i++; + } +} + +unsigned +DataValidatorGroup::failover_count() +{ + return _toggle_count; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h new file mode 100644 index 0000000000..f35e0d71c1 --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 data_validator_group.h + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "data_validator.h" + +class __EXPORT DataValidatorGroup { +public: + DataValidatorGroup(unsigned siblings); + virtual ~DataValidatorGroup(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, + float val[3], uint64_t error_count, int priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float* get_best(uint64_t timestamp, int *index); + + /** + * Get the RMS / vibration factor + * + * @return float value representing the RMS, which a valid indicator for vibration + */ + float get_vibration_factor(uint64_t timestamp); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count(); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint64_t timeout_interval_us); + +private: + DataValidator *_first; /**< sibling in the group */ + int _curr_best; /**< currently best index */ + int _prev_best; /**< the previous best index */ + uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ + unsigned _toggle_count; /**< number of back and forth switches between two sensors */ + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup&); + DataValidatorGroup operator=(const DataValidatorGroup&); +}; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a320ffe518..14f5470425 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_timestamp[0] != raw.timestamp) { + if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; + sensor_last_timestamp[0] = raw.gyro_timestamp[0]; } z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; @@ -394,10 +394,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; } hrt_abstime vel_t = 0; @@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && /* check that the mag vector is > 0 */ fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.magnetometer_timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp[0]; } bool vision_updated = false; diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 20eda27690..716b0aefe4 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACXXFLAGS = -Wframe-larger-than=2400 +EXTRACXXFLAGS = -Wframe-larger-than=2600 endif diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 138a8c7e5c..9b60701413 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -59,8 +58,11 @@ #include #include -#include +#include +#include #include +#include +#include #include #include @@ -103,6 +105,8 @@ public: void task_main(); + void print(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -121,6 +125,7 @@ private: param_t mag_decl_auto; param_t acc_comp; param_t bias_max; + param_t vibe_thresh; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; @@ -130,6 +135,8 @@ private: bool _mag_decl_auto = false; bool _acc_comp = false; float _bias_max = 0.0f; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; Vector<3> _gyro; Vector<3> _accel; @@ -142,10 +149,23 @@ private: vehicle_global_position_s _gpos = {}; Vector<3> _vel_prev; Vector<3> _pos_acc; + + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + hrt_abstime _vel_prev_t = 0; bool _inited = false; bool _data_good = false; + bool _failsafe = false; + bool _vibration_warning = false; + + int _mavlink_fd = -1; perf_counter_t _update_perf; perf_counter_t _loop_perf; @@ -160,7 +180,15 @@ private: }; -AttitudeEstimatorQ::AttitudeEstimatorQ() { +AttitudeEstimatorQ::AttitudeEstimatorQ() : + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _lp_roll_rate(250.0f, 20.0f), + _lp_pitch_rate(250.0f, 20.0f) +{ + _voter_mag.set_timeout(200000); + _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); @@ -168,12 +196,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() { _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); + _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); } /** * Destructor, also kills task. */ -AttitudeEstimatorQ::~AttitudeEstimatorQ() { +AttitudeEstimatorQ::~AttitudeEstimatorQ() +{ if (_control_task != -1) { /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; @@ -196,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { attitude_estimator_q::instance = nullptr; } -int AttitudeEstimatorQ::start() { +int AttitudeEstimatorQ::start() +{ ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("attitude_estimator_q", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 2100, (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); @@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() { return OK; } -void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { +void AttitudeEstimatorQ::print() +{ + warnx("gyro status:"); + _voter_gyro.print(); + warnx("accel status:"); + _voter_accel.print(); + warnx("mag status:"); + _voter_mag.print(); +} + +void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) +{ attitude_estimator_q::instance->task_main(); } -void AttitudeEstimatorQ::task_main() { - +void AttitudeEstimatorQ::task_main() +{ _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -236,6 +278,10 @@ void AttitudeEstimatorQ::task_main() { while (!_task_should_exit) { int ret = px4_poll(fds, 1, 1000); + if (_mavlink_fd < 0) { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + if (ret < 0) { // Poll error, sleep and try again usleep(10000); @@ -250,11 +296,71 @@ void AttitudeEstimatorQ::task_main() { // Update sensors sensor_combined_s sensors; if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { - _gyro.set(sensors.gyro_rad_s); - _accel.set(sensors.accelerometer_m_s2); - _mag.set(sensors.magnetometer_ga); + // Feed validator with recent sensor data + + for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { + + /* ignore empty fields */ + if (sensors.gyro_timestamp[i] > 0) { + + float gyro[3]; + + for (unsigned j = 0; j < 3; j++) { + if (sensors.gyro_integral_dt[i] > 0) { + gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); + } else { + /* fall back to angular rate */ + gyro[j] = sensors.gyro_rad_s[i * 3 + j]; + } + } + + _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); + } + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } + + int best_gyro, best_accel, best_mag; + + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + _gyro.set(_voter_gyro.get_best(curr_time, &best_gyro)); + _accel.set(_voter_accel.get_best(curr_time, &best_accel)); + _mag.set(_voter_mag.get_best(curr_time, &best_mag)); + + if (_accel.length() < 0.01f || _mag.length() < 0.01f) { + warnx("WARNING: degenerate accel / mag!"); + continue; + } _data_good = true; + + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { + + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + } else { + _vibration_warning_timestamp = 0; + } } bool gpos_updated; @@ -311,8 +417,11 @@ void AttitudeEstimatorQ::task_main() { att.pitch = euler(1); att.yaw = euler(2); - att.rollspeed = _rates(0); - att.pitchspeed = _rates(1); + /* the complimentary filter should reflect the true system + * state, but we need smoother outputs for the control system + */ + att.rollspeed = _lp_roll_rate.apply(_rates(0)); + att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); att.yawspeed = _rates(2); for (int i = 0; i < 3; i++) { @@ -328,6 +437,10 @@ void AttitudeEstimatorQ::task_main() { memcpy(&att.R[0], R.data, sizeof(att.R)); att.R_valid = true; + att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); + att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); + att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); + if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { @@ -358,6 +471,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) { param_get(_params_handles.acc_comp, &acc_comp_int); _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); + param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); } } @@ -501,6 +615,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { + attitude_estimator_q::instance->print(); warnx("running"); return 0; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index 730d7a4e8b..d4d9d10eb4 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -108,3 +108,12 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); * @unit rad/s */ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); + +/** + * Threshold (of RMS) to warn about high vibration levels + * + * @group Attitude Q estimator + * @min 0.001 + * @max 100 + */ +PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f9477d6f7f..f9fdd6dadb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1438,7 +1438,7 @@ int commander_thread_main(int argc, char *argv[]) * vertical separation from other airtraffic the operator has to know when the * barometer is inoperational. * */ - if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status.barometer_failure) { status.barometer_failure = false; diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index c16a606711..e4fcea2c69 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -55,6 +55,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACXXFLAGS = -Wframe-larger-than=2200 +EXTRACXXFLAGS = -Wframe-larger-than=2400 endif diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 532a037d40..80f1bae1b0 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -97,6 +97,11 @@ void BlockParam::update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } +template +void BlockParam::commit() { + if (_handle != PARAM_INVALID) param_set(_handle, &_val); +} + template BlockParam::~BlockParam() {}; diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index cab28c65fd..db035f9f91 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -79,6 +79,7 @@ public: BlockParam(Block *block, const char *name, bool parent_prefix = true); T get(); + void commit(); void set(T val); void update(); virtual ~BlockParam(); diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 28b23f2069..e4d924396a 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -70,11 +69,16 @@ #include #include +#include +#include "estimator_22states.h" + +#include +#include //Forward declaration class AttPosEKF; -class AttitudePositionEstimatorEKF +class AttitudePositionEstimatorEKF : public control::SuperBlock { public: /** @@ -168,9 +172,8 @@ private: struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; - struct gyro_scale _gyro_offsets[3]; - struct accel_scale _accel_offsets[3]; - struct mag_scale _mag_offsets[3]; + hrt_abstime _last_accel; + hrt_abstime _last_mag; struct sensor_combined_s _sensor_combined; @@ -179,6 +182,8 @@ private: float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; + float _vibration_warning_threshold = 1.0f; + hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -198,14 +203,16 @@ private: bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; - hrt_abstime _last_run; hrt_abstime _distance_last_valid; - bool _gyro_valid; - bool _accel_valid; - bool _mag_valid; + DataValidatorGroup _voter_gyro; + DataValidatorGroup _voter_accel; + DataValidatorGroup _voter_mag; int _gyro_main; ///< index of the main gyroscope int _accel_main; ///< index of the main accelerometer int _mag_main; ///< index of the main magnetometer + bool _data_good; ///< all required filter data is ok + bool _failsafe; ///< failsafe on one of the sensors + bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 @@ -216,6 +223,10 @@ private: int _mavlink_fd; + control::BlockParamFloat _mag_offset_x; + control::BlockParamFloat _mag_offset_y; + control::BlockParamFloat _mag_offset_z; + struct { int32_t vel_delay_ms; int32_t pos_delay_ms; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5f3634d22b..c75a9e547d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -68,7 +68,6 @@ #include #include -static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; //Constants @@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f; */ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); -__EXPORT uint32_t millis(); - -__EXPORT uint64_t getMicros(); +uint32_t millis(); +uint64_t getMicros(); +uint32_t getMillis(); uint32_t millis() { - return IMUmsec; + return getMillis(); +} + +uint32_t getMillis() +{ + return getMicros() / 1000; } uint64_t getMicros() @@ -115,6 +119,7 @@ AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : + SuperBlock(NULL, "EKF"), _task_should_exit(false), _task_running(false), _estimator_task(-1), @@ -133,84 +138,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _landDetectorSub(-1), _armedSub(-1), -/* publications */ + /* publications */ _att_pub(nullptr), _global_pos_pub(nullptr), _local_pos_pub(nullptr), _estimator_status_pub(nullptr), _wind_pub(nullptr), - _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance {}, - _landDetector {}, - _armed {}, + _att{}, + _gyro{}, + _accel{}, + _mag{}, + _airspeed{}, + _baro{}, + _vstatus{}, + _global_pos{}, + _local_pos{}, + _gps{}, + _wind{}, + _distance{}, + _landDetector{}, + _armed{}, - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), + _last_accel(0), + _last_mag(0), - _sensor_combined {}, + _sensor_combined{}, - _pos_ref{}, - _filter_ref_offset(0.0f), - _baro_gps_offset(0.0f), + _pos_ref{}, + _filter_ref_offset(0.0f), + _baro_gps_offset(0.0f), - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(hrt_absolute_time()), + _distance_last_valid(0), + _voter_gyro(3), + _voter_accel(3), + _voter_mag(3), + _gyro_main(-1), + _accel_main(-1), + _mag_main(-1), + _data_good(false), + _failsafe(false), + _vibration_warning(false), + _ekf_logging(true), + _debug(0), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), - _mavlink_fd(-1), - _parameters{}, - _parameter_handles{}, - _ekf(nullptr), + _mavlink_fd(-1), + _mag_offset_x(this, "PE_MAGB_X"), + _mag_offset_y(this, "PE_MAGB_Y"), + _mag_offset_z(this, "PE_MAGB_Z"), + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), - _LP_att_P(100.0f, 10.0f), - _LP_att_Q(100.0f, 10.0f), - _LP_att_R(100.0f, 10.0f) + _LP_att_P(250.0f, 20.0f), + _LP_att_Q(250.0f, 20.0f), + _LP_att_R(250.0f, 20.0f) { - _last_run = hrt_absolute_time(); + _voter_mag.set_timeout(200000); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -237,48 +246,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : /* fetch initial parameter values */ parameters_update(); - - /* get offsets */ - int fd, res; - - for (unsigned s = 0; s < 3; s++) { - char str[30]; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("G%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("A%u SCALE FAIL", s); - } - } - - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - fd = px4_open(str, O_RDONLY); - - if (fd >= 0) { - res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]); - px4_close(fd); - - if (res) { - PX4_WARN("M%u SCALE FAIL", s); - } - } - } } AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() @@ -317,6 +284,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging) int AttitudePositionEstimatorEKF::parameters_update() { + /* update C++ param system */ + updateParams(); param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); @@ -354,6 +323,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF + #if 0 + // Initially disable loading until + // convergence is flight-test proven + _ekf->magBias.x = _mag_offset_x.get(); + _ekf->magBias.y = _mag_offset_y.get(); + _ekf->magBias.z = _mag_offset_z.get(); + #endif } return OK; @@ -366,12 +342,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); + bool landed = _vstatus.condition_landed; + if (vstatus_updated) { orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - //Tell EKF that the vehicle is a fixed wing or multi-rotor + // Tell EKF that the vehicle is a fixed wing or multi-rotor _ekf->setIsFixedWing(!_vstatus.is_rotary_wing); + + // Save params on landed + if (!landed && _vstatus.condition_landed) { + _mag_offset_x.set(_ekf->magBias.x); + _mag_offset_x.commit(); + _mag_offset_y.set(_ekf->magBias.y); + _mag_offset_y.commit(); + _mag_offset_z.set(_ekf->magBias.z); + _mag_offset_z.commit(); + } } } @@ -541,8 +529,6 @@ void AttitudePositionEstimatorEKF::task_main() orb_set_interval(_vstatus_sub, 200); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 9); /* sets also parameters in the EKF object */ parameters_update(); @@ -610,15 +596,14 @@ void AttitudePositionEstimatorEKF::task_main() orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; + _gyro_main = -1; + _accel_main = -1; + _mag_main = -1; _baro_init = false; _gps_initialized = false; _last_sensor_timestamp = hrt_absolute_time(); - _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; @@ -646,7 +631,7 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high @@ -836,9 +821,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; @@ -1009,7 +994,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->UpdateStrapdownEquationsNED(); // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); + _ekf->StoreStates(getMillis()); // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; @@ -1035,8 +1020,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1059,8 +1044,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1076,7 +1061,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms)); // run the fusion step _ekf->FuseVelposNED(); @@ -1089,7 +1074,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseMag) { _ekf->fuseMagData = true; _ekf->RecallStates(_ekf->statesAtMagMeasTime, - (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); @@ -1104,7 +1089,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, - (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); } else { @@ -1117,7 +1102,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // _ekf->rngMea is set in sensor readout already _ekf->fuseRngData = true; _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f)); _ekf->OpticalFlowEKF(); _ekf->fuseRngData = false; } @@ -1131,7 +1116,7 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_MAX - 20, 4800, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); @@ -1163,7 +1148,7 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis()); PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, @@ -1210,6 +1195,13 @@ void AttitudePositionEstimatorEKF::print_status() (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + + PX4_INFO("gyro status:"); + _voter_gyro.print(); + PX4_INFO("accel status:"); + _voter_accel.print(); + PX4_INFO("mag status:"); + _voter_mag.print(); } void AttitudePositionEstimatorEKF::pollData() @@ -1222,125 +1214,153 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); } - //Update Gyro and Accelerometer - static Vector3f lastAngRate; - static Vector3f lastAccel; - bool accel_updated = false; - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - static hrt_abstime last_accel = 0; - static hrt_abstime last_mag = 0; + // Feed validator with recent sensor data - if (last_accel != _sensor_combined.accelerometer_timestamp) { - accel_updated = true; - - } else { - accel_updated = false; + for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { + _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } - last_accel = _sensor_combined.accelerometer_timestamp; + // Get best measurement values + hrt_abstime curr_time = hrt_absolute_time(); + (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { + // Use pre-integrated values if possible + if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) { + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]); + _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]); + _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]); + } + + _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]; + perf_count(_perf_gyro); + } + + (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { + + // Use pre-integrated values if possible + if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) { + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { + _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); + _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); + } + + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]; + _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + } + + (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { + Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + + /* fail over to the 2nd mag if we know the first is down */ + if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { + _ekf->magData.x = mag.x; + _ekf->magData.y = mag.y; + _ekf->magData.z = mag.z; + _newDataMag = true; + _last_mag = _sensor_combined.magnetometer_timestamp[_mag_main]; + perf_count(_perf_mag); + } + } + + if (!_failsafe && (_voter_gyro.failover_count() > 0 || + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { + + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } + + if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { + _vibration_warning = true; + mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + } + } else { + _vibration_warning_timestamp = 0; + } - // Copy gyro and accel - _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3; IMUusec = _sensor_combined.timestamp; - float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; + _last_sensor_timestamp = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } - _last_run = _sensor_combined.timestamp; - // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - int last_gyro_main = _gyro_main; + // XXX this is for assessing the filter performance + // leave this in as long as larger improvements are still being made. + #if 0 - if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) { + float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f; + float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - _gyro_main = 0; - _gyro_valid = true; + static unsigned dtoverflow5 = 0; + static unsigned dtoverflow10 = 0; + static hrt_abstime lastprint = 0; - } else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) && - PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) { + if (hrt_elapsed_time(&lastprint) > 1000000) { + warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); - _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro1_rad_s[2]; - _gyro_main = 1; - _gyro_valid = true; + warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z); - } else { - _gyro_valid = false; + warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + + lastprint = hrt_absolute_time(); } - if (last_gyro_main != _gyro_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main); + if (deltaT > 0.005f) { + dtoverflow5++; } - if (!_gyro_valid) { - /* keep last value if he hit an out of band value */ - lastAngRate = _ekf->angRate; - - } else { - perf_count(_perf_gyro); + if (deltaT > 0.01f) { + dtoverflow10++; } + #endif - if (accel_updated) { - - int last_accel_main = _accel_main; - - /* fail over to the 2nd accel if we know the first is down */ - if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) { - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - _accel_main = 0; - - } else { - _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2]; - _accel_main = 1; - } - - if (!_accel_valid) { - lastAccel = _ekf->accel; - } - - if (last_accel_main != _accel_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main); - } - - _accel_valid = true; - } - - _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; - lastAngRate = _ekf->angRate; - _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; - lastAccel = _ekf->accel; - - if (last_mag != _sensor_combined.magnetometer_timestamp) { - _newDataMag = true; - - } else { - _newDataMag = false; - } - - last_mag = _sensor_combined.magnetometer_timestamp; + _data_good = true; //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); @@ -1362,7 +1382,6 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } - bool gps_update; orb_check(_gps_sub, &gps_update); @@ -1516,57 +1535,6 @@ void AttitudePositionEstimatorEKF::pollData() perf_count(_perf_baro); } - //Update Magnetometer - if (_newDataMag) { - - _mag_valid = true; - - perf_count(_perf_mag); - - int last_mag_main = _mag_main; - - Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], - _sensor_combined.magnetometer_ga[2]); - - Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1], - _sensor_combined.magnetometer1_ga[2]); - - const unsigned mag_timeout_us = 200000; - - /* fail over to the 2nd mag if we know the first is down */ - if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us && - _sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) && - mag0.length() > 0.1f) { - _ekf->magData.x = mag0.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag0.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag0.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 0; - - } else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us && - mag1.length() > 0.1f) { - _ekf->magData.x = mag1.x; - _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - - _ekf->magData.y = mag1.y; - _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - - _ekf->magData.z = mag1.z; - _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset - _mag_main = 1; - } else { - _mag_valid = false; - } - - if (last_mag_main != _mag_main) { - mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main); - } - } - //Update range data orb_check(_distance_sub, &_newRangeData); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 943365e499..0d33e28c91 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -258,6 +258,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Magnetometer X bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f); + +/** + * Magnetometer Y bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f); + +/** + * Magnetometer Z bias + * + * The magnetometer bias. This bias is learnt by the filter over time and + * persists between boots. + * + * @min -0.6 + * @max 0.6 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f); + /** * Threshold for filter initialization. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2829b97cf3..2068147fe7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -643,28 +643,28 @@ protected: if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp) { + if (_accel_timestamp != sensor.accelerometer_timestamp[0]) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp; + _accel_timestamp = sensor.accelerometer_timestamp[0]; } - if (_gyro_timestamp != sensor.timestamp) { + if (_gyro_timestamp != sensor.gyro_timestamp[0]) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.timestamp; + _gyro_timestamp = sensor.gyro_timestamp[0]; } - if (_mag_timestamp != sensor.magnetometer_timestamp) { + if (_mag_timestamp != sensor.magnetometer_timestamp[0]) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp; + _mag_timestamp = sensor.magnetometer_timestamp[0]; } - if (_baro_timestamp != sensor.baro_timestamp) { + if (_baro_timestamp != sensor.baro_timestamp[0]) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp; + _baro_timestamp = sensor.baro_timestamp[0]; } mavlink_highres_imu_t msg; @@ -679,10 +679,10 @@ protected: msg.xmag = sensor.magnetometer_ga[0]; msg.ymag = sensor.magnetometer_ga[1]; msg.zmag = sensor.magnetometer_ga[2]; - msg.abs_pressure = sensor.baro_pres_mbar; - msg.diff_pressure = sensor.differential_pressure_pa; - msg.pressure_alt = sensor.baro_alt_meter; - msg.temperature = sensor.baro_temp_celcius; + msg.abs_pressure = sensor.baro_pres_mbar[0]; + msg.diff_pressure = sensor.differential_pressure_pa[0]; + msg.pressure_alt = sensor.baro_alt_meter[0]; + msg.temperature = sensor.baro_temp_celcius[0]; msg.fields_updated = fields_updated; _mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 58124f2133..2eae087010 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1410,6 +1410,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_timestamp[0] = timestamp; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -1417,9 +1418,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[0] = imu.xacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_timestamp = timestamp; + hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 + hil_sensors.accelerometer_timestamp[0] = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -1431,19 +1432,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_timestamp = timestamp; + hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16 + hil_sensors.magnetometer_mode[0] = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f; + hil_sensors.magnetometer_timestamp[0] = timestamp; - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_timestamp = timestamp; + hil_sensors.baro_pres_mbar[0] = imu.abs_pressure; + hil_sensors.baro_alt_meter[0] = imu.pressure_alt; + hil_sensors.baro_temp_celcius[0] = imu.temperature; + hil_sensors.baro_timestamp[0] = timestamp; - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa; - hil_sensors.differential_pressure_timestamp = timestamp; + hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0]; + hil_sensors.differential_pressure_timestamp[0] = timestamp; /* publish combined sensor topic */ if (_sensors_pub == nullptr) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 454326a923..50b8746a71 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -406,7 +406,7 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; if (!inside) { 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 892e50f3a1..ee8359f0c3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -399,13 +399,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_timestamp != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp; + if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp[0]; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(sensor.baro_alt_meter)) { - baro_offset += sensor.baro_alt_meter; + if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { + baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } @@ -474,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_timestamp != accel_timestamp) { + if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -496,13 +496,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp; + accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } - if (sensor.baro_timestamp != baro_timestamp) { - corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_timestamp = sensor.baro_timestamp; + if (sensor.baro_timestamp[0] != baro_timestamp) { + corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; + baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d22d0a9d93..69afb39099 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os ifeq ($(PX4_TARGET_OS),nuttx) -EXTRACFLAGS = -Wframe-larger-than=1400 +EXTRACFLAGS = -Wframe-larger-than=1600 endif diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a81ab9fdb1..253949b26e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1287,18 +1287,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - hrt_abstime gyro_timestamp = 0; - hrt_abstime accelerometer_timestamp = 0; - hrt_abstime magnetometer_timestamp = 0; - hrt_abstime barometer_timestamp = 0; - hrt_abstime differential_pressure_timestamp = 0; - hrt_abstime barometer1_timestamp = 0; - hrt_abstime gyro1_timestamp = 0; - hrt_abstime accelerometer1_timestamp = 0; - hrt_abstime magnetometer1_timestamp = 0; - hrt_abstime gyro2_timestamp = 0; - hrt_abstime accelerometer2_timestamp = 0; - hrt_abstime magnetometer2_timestamp = 0; + hrt_abstime gyro_timestamp[3] = {0, 0, 0}; + hrt_abstime accelerometer_timestamp[3] = {0, 0, 0}; + hrt_abstime magnetometer_timestamp[3] = {0, 0, 0}; + hrt_abstime barometer_timestamp[3] = {0, 0, 0}; + hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0}; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1446,144 +1439,86 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SENSOR COMBINED --- */ if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) { - bool write_IMU = false; - bool write_IMU1 = false; - bool write_IMU2 = false; - bool write_SENS = false; - bool write_SENS1 = false; + - if (buf.sensor.timestamp != gyro_timestamp) { - gyro_timestamp = buf.sensor.timestamp; - write_IMU = true; + for (unsigned i = 0; i < 3; i++) { + bool write_IMU = false; + bool write_SENS = false; + + if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) { + gyro_timestamp[i] = buf.sensor.gyro_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) { + accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) { + magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i]; + write_IMU = true; + } + + if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) { + barometer_timestamp[i] = buf.sensor.baro_timestamp[i]; + write_SENS = true; + } + + if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) { + differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i]; + write_SENS = true; + } + + if (write_IMU) { + switch (i) { + case 0: + log_msg.msg_type = LOG_IMU_MSG; + break; + case 1: + log_msg.msg_type = LOG_IMU1_MSG; + break; + case 2: + log_msg.msg_type = LOG_IMU2_MSG; + break; + } + + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2]; + log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0]; + log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (write_SENS) { + switch (i) { + case 0: + log_msg.msg_type = LOG_SENS_MSG; + break; + case 1: + log_msg.msg_type = LOG_AIR1_MSG; + break; + case 2: + continue; + break; + } + + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i]; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i]; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i]; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i]; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i]; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } } - - if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { - accelerometer_timestamp = buf.sensor.accelerometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { - magnetometer_timestamp = buf.sensor.magnetometer_timestamp; - write_IMU = true; - } - - if (buf.sensor.baro_timestamp != barometer_timestamp) { - barometer_timestamp = buf.sensor.baro_timestamp; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { - differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; - write_SENS = true; - } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - - if (buf.sensor.baro1_timestamp != barometer1_timestamp) { - barometer1_timestamp = buf.sensor.baro1_timestamp; - write_SENS1 = true; - } - - if (write_SENS1) { - log_msg.msg_type = LOG_AIR1_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; - log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; - // XXX moving to AIR0-AIR2 instead of SENS - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - - if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { - accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; - write_IMU1 = true; - } - - if (buf.sensor.gyro1_timestamp != gyro1_timestamp) { - gyro1_timestamp = buf.sensor.gyro1_timestamp; - write_IMU1 = true; - } - - if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) { - magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp; - write_IMU1 = true; - } - - if (write_IMU1) { - log_msg.msg_type = LOG_IMU1_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) { - accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp; - write_IMU2 = true; - } - - if (buf.sensor.gyro2_timestamp != gyro2_timestamp) { - gyro2_timestamp = buf.sensor.gyro2_timestamp; - write_IMU2 = true; - } - - if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) { - magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp; - write_IMU2 = true; - } - - if (write_IMU2) { - log_msg.msg_type = LOG_IMU2_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2]; - log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp; - log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp; - log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - } /* --- ATTITUDE --- */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26f7b1da30..86df382df8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -129,6 +129,8 @@ #define STICK_ON_OFF_LIMIT 0.75f #define MAG_ROT_VAL_INTERNAL -1 +#define SENSOR_COUNT_MAX 3 + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -200,25 +202,23 @@ private: bool _hil_enabled; /**< if true, HIL is active */ bool _publishing; /**< if true, we are publishing sensor data */ + bool _armed; /**< arming status of the vehicle */ - int _gyro_sub; /**< raw gyro0 data subscription */ - int _accel_sub; /**< raw accel0 data subscription */ - int _mag_sub; /**< raw mag0 data subscription */ - int _gyro1_sub; /**< raw gyro1 data subscription */ - int _accel1_sub; /**< raw accel1 data subscription */ - int _mag1_sub; /**< raw mag1 data subscription */ - int _gyro2_sub; /**< raw gyro2 data subscription */ - int _accel2_sub; /**< raw accel2 data subscription */ - int _mag2_sub; /**< raw mag2 data subscription */ + int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */ + int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */ + int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */ + int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */ + unsigned _gyro_count; /**< raw gyro data count */ + unsigned _accel_count; /**< raw accel data count */ + unsigned _mag_count; /**< raw mag data count */ + unsigned _baro_count; /**< raw baro data count */ + int _rc_sub; /**< raw rc channels data subscription */ - int _baro_sub; /**< raw baro0 data subscription */ - int _baro1_sub; /**< raw baro1 data subscription */ - //int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vcontrol_mode_sub; /**< vehicle control mode subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ - int _rc_parameter_map_sub; /**< rc parameter map subscription */ - int _manual_control_sub; /**< notification of manual control updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ + int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ @@ -228,18 +228,6 @@ private: orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ - int32_t _gyro_prio; /**< gyro0 sensor priority */ - int32_t _accel_prio; /**< accel0 sensor priority */ - int32_t _mag_prio; /**< mag0 sensor priority */ - int32_t _gyro1_prio; /**< gyro1 sensor priority */ - int32_t _accel1_prio; /** _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -377,6 +365,9 @@ private: } _parameter_handles; /**< handles for interesting parameters */ + int init_sensor_class(const struct orb_metadata *meta, int *subs, + unsigned *priorities, unsigned *errcount); + /** * Update our local parameter cache. */ @@ -495,20 +486,18 @@ Sensors::Sensors() : _sensors_task(-1), _hil_enabled(false), _publishing(true), + _armed(false), /* subscriptions */ - _gyro_sub(-1), - _accel_sub(-1), - _mag_sub(-1), - _gyro1_sub(-1), - _accel1_sub(-1), - _mag1_sub(-1), - _gyro2_sub(-1), - _accel2_sub(-1), - _mag2_sub(-1), + _gyro_sub{-1, -1, -1}, + _accel_sub{-1, -1, -1}, + _mag_sub{-1, -1, -1}, + _baro_sub{-1, -1, -1}, + _gyro_count(0), + _accel_count(0), + _mag_count(0), + _baro_count(0), _rc_sub(-1), - _baro_sub(-1), - _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), @@ -523,19 +512,6 @@ Sensors::Sensors() : _airspeed_pub(nullptr), _diff_pres_pub(nullptr), - /* sensor priorities */ - _gyro_prio(-1), - _accel_prio(-1), - _mag_prio(-1), - _gyro1_prio(-1), - _accel1_prio(-1), - _mag1_prio(-1), - _gyro2_prio(-1), - _accel2_prio(-1), - _mag2_prio(-1), - _baro_prio(-1), - _baro1_prio(-1), - /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), @@ -546,6 +522,14 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + /* initialize subscriptions */ + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + _gyro_sub[i] = -1; + _accel_sub[i] = -1; + _mag_sub[i] = -1; + _baro_sub[i] = -1; + } + memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); @@ -1032,269 +1016,132 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + for (unsigned i = 0; i < _accel_count; i++) { + bool accel_updated; + orb_check(_accel_sub[i], &accel_updated); - if (accel_updated) { - struct accel_report accel_report; + if (accel_updated) { + struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; - raw.accelerometer_m_s2[0] = vect(0); - raw.accelerometer_m_s2[1] = vect(1); - raw.accelerometer_m_s2[2] = vect(2); + raw.accelerometer_m_s2[i * 3 + 0] = vect(0); + raw.accelerometer_m_s2[i * 3 + 1] = vect(1); + raw.accelerometer_m_s2[i * 3 + 2] = vect(2); - raw.accelerometer_raw[0] = accel_report.x_raw; - raw.accelerometer_raw[1] = accel_report.y_raw; - raw.accelerometer_raw[2] = accel_report.z_raw; + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.accelerometer_timestamp = accel_report.timestamp; - raw.accelerometer_priority = _accel_prio; - raw.accelerometer_errcount = accel_report.error_count; - raw.accelerometer_temp = accel_report.temperature; - } + raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0); + raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1); + raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2); - orb_check(_accel1_sub, &accel_updated); + raw.accelerometer_integral_dt[i] = accel_report.integral_dt; - if (accel_updated) { - struct accel_report accel_report; + raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw; + raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw; + raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw; - orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer1_m_s2[0] = vect(0); - raw.accelerometer1_m_s2[1] = vect(1); - raw.accelerometer1_m_s2[2] = vect(2); - - raw.accelerometer1_raw[0] = accel_report.x_raw; - raw.accelerometer1_raw[1] = accel_report.y_raw; - raw.accelerometer1_raw[2] = accel_report.z_raw; - - raw.accelerometer1_timestamp = accel_report.timestamp; - raw.accelerometer1_priority = _accel1_prio; - raw.accelerometer1_errcount = accel_report.error_count; - raw.accelerometer1_temp = accel_report.temperature; - } - - orb_check(_accel2_sub, &accel_updated); - - if (accel_updated) { - struct accel_report accel_report; - - orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); - - math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); - vect = _board_rotation * vect; - - raw.accelerometer2_m_s2[0] = vect(0); - raw.accelerometer2_m_s2[1] = vect(1); - raw.accelerometer2_m_s2[2] = vect(2); - - raw.accelerometer2_raw[0] = accel_report.x_raw; - raw.accelerometer2_raw[1] = accel_report.y_raw; - raw.accelerometer2_raw[2] = accel_report.z_raw; - - raw.accelerometer2_timestamp = accel_report.timestamp; - raw.accelerometer2_priority = _accel2_prio; - raw.accelerometer2_errcount = accel_report.error_count; - raw.accelerometer2_temp = accel_report.temperature; + raw.accelerometer_timestamp[i] = accel_report.timestamp; + raw.accelerometer_errcount[i] = accel_report.error_count; + raw.accelerometer_temp[i] = accel_report.temperature; + } } } void Sensors::gyro_poll(struct sensor_combined_s &raw) { - bool gyro_updated; - orb_check(_gyro_sub, &gyro_updated); + for (unsigned i = 0; i < _gyro_count; i++) { + bool gyro_updated; + orb_check(_gyro_sub[i], &gyro_updated); - if (gyro_updated) { - struct gyro_report gyro_report; + if (gyro_updated) { + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; - raw.gyro_rad_s[0] = vect(0); - raw.gyro_rad_s[1] = vect(1); - raw.gyro_rad_s[2] = vect(2); + raw.gyro_rad_s[i * 3 + 0] = vect(0); + raw.gyro_rad_s[i * 3 + 1] = vect(1); + raw.gyro_rad_s[i * 3 + 2] = vect(2); - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; - raw.timestamp = gyro_report.timestamp; - raw.gyro_priority = _gyro_prio; - raw.gyro_errcount = gyro_report.error_count; - raw.gyro_temp = gyro_report.temperature; - } + raw.gyro_integral_rad[i * 3 + 0] = vect_int(0); + raw.gyro_integral_rad[i * 3 + 1] = vect_int(1); + raw.gyro_integral_rad[i * 3 + 2] = vect_int(2); - orb_check(_gyro1_sub, &gyro_updated); + raw.gyro_integral_dt[i] = gyro_report.integral_dt; - if (gyro_updated) { - struct gyro_report gyro_report; + raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw; + raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw; + raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; - orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro1_rad_s[0] = vect(0); - raw.gyro1_rad_s[1] = vect(1); - raw.gyro1_rad_s[2] = vect(2); - - raw.gyro1_raw[0] = gyro_report.x_raw; - raw.gyro1_raw[1] = gyro_report.y_raw; - raw.gyro1_raw[2] = gyro_report.z_raw; - - raw.gyro1_timestamp = gyro_report.timestamp; - raw.gyro1_priority = _gyro1_prio; - raw.gyro1_errcount = gyro_report.error_count; - raw.gyro1_temp = gyro_report.temperature; - } - - orb_check(_gyro2_sub, &gyro_updated); - - if (gyro_updated) { - struct gyro_report gyro_report; - - orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); - - math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); - vect = _board_rotation * vect; - - raw.gyro2_rad_s[0] = vect(0); - raw.gyro2_rad_s[1] = vect(1); - raw.gyro2_rad_s[2] = vect(2); - - raw.gyro2_raw[0] = gyro_report.x_raw; - raw.gyro2_raw[1] = gyro_report.y_raw; - raw.gyro2_raw[2] = gyro_report.z_raw; - - raw.gyro2_timestamp = gyro_report.timestamp; - raw.gyro2_priority = _gyro2_prio; - raw.gyro2_errcount = gyro_report.error_count; - raw.gyro2_temp = gyro_report.temperature; + raw.gyro_timestamp[i] = gyro_report.timestamp; + if (i == 0) { + raw.timestamp = gyro_report.timestamp; + } + raw.gyro_errcount[i] = gyro_report.error_count; + raw.gyro_temp[i] = gyro_report.temperature; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - bool mag_updated; - orb_check(_mag_sub, &mag_updated); + for (unsigned i = 0; i < _mag_count; i++) { + bool mag_updated; + orb_check(_mag_sub[i], &mag_updated); - if (mag_updated) { - struct mag_report mag_report; + if (mag_updated) { + struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report); - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - vect = _mag_rotation[0] * vect; + vect = _mag_rotation[i] * vect; - raw.magnetometer_ga[0] = vect(0); - raw.magnetometer_ga[1] = vect(1); - raw.magnetometer_ga[2] = vect(2); + raw.magnetometer_ga[i * 3 + 0] = vect(0); + raw.magnetometer_ga[i * 3 + 1] = vect(1); + raw.magnetometer_ga[i * 3 + 2] = vect(2); - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; + raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw; + raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw; + raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw; - raw.magnetometer_timestamp = mag_report.timestamp; - raw.magnetometer_priority = _mag_prio; - raw.magnetometer_errcount = mag_report.error_count; - raw.magnetometer_temp = mag_report.temperature; - } - - orb_check(_mag1_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[1] * vect; - - raw.magnetometer1_ga[0] = vect(0); - raw.magnetometer1_ga[1] = vect(1); - raw.magnetometer1_ga[2] = vect(2); - - raw.magnetometer1_raw[0] = mag_report.x_raw; - raw.magnetometer1_raw[1] = mag_report.y_raw; - raw.magnetometer1_raw[2] = mag_report.z_raw; - - raw.magnetometer1_timestamp = mag_report.timestamp; - raw.magnetometer1_priority = _mag1_prio; - raw.magnetometer1_errcount = mag_report.error_count; - raw.magnetometer1_temp = mag_report.temperature; - } - - orb_check(_mag2_sub, &mag_updated); - - if (mag_updated) { - struct mag_report mag_report; - - orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); - - math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - - vect = _mag_rotation[2] * vect; - - raw.magnetometer2_ga[0] = vect(0); - raw.magnetometer2_ga[1] = vect(1); - raw.magnetometer2_ga[2] = vect(2); - - raw.magnetometer2_raw[0] = mag_report.x_raw; - raw.magnetometer2_raw[1] = mag_report.y_raw; - raw.magnetometer2_raw[2] = mag_report.z_raw; - - raw.magnetometer2_timestamp = mag_report.timestamp; - raw.magnetometer2_priority = _mag2_prio; - raw.magnetometer2_errcount = mag_report.error_count; - raw.magnetometer2_temp = mag_report.temperature; + raw.magnetometer_timestamp[i] = mag_report.timestamp; + raw.magnetometer_errcount[i] = mag_report.error_count; + raw.magnetometer_temp[i] = mag_report.temperature; + } } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + for (unsigned i = 0; i < _baro_count; i++) { + bool baro_updated; + orb_check(_baro_sub[i], &baro_updated); - if (baro_updated) { + if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); - raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar - raw.baro_alt_meter = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius - raw.baro_timestamp = _barometer.timestamp; - raw.baro_priority = _baro_prio; - } - - orb_check(_baro1_sub, &baro_updated); - - if (baro_updated) { - - struct baro_report baro_report; - - orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); - - raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - - raw.baro1_timestamp = baro_report.timestamp; - raw.baro1_priority = _baro1_prio; + raw.baro_timestamp[i] = _barometer.timestamp; + } } } @@ -1307,12 +1154,12 @@ 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_raw_pa; - raw.differential_pressure_timestamp = _diff_pres.timestamp; - raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp[0] = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa[0] = _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); + (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; @@ -1320,11 +1167,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _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)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1352,16 +1199,17 @@ Sensors::vehicle_control_mode_poll() orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ - //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; + _armed = vcontrol_mode.flag_armed; /* switching from HIL to non-HIL mode */ } else if (!_publishing && !_hil_enabled) { _hil_enabled = false; _publishing = true; + _armed = vcontrol_mode.flag_armed; } } } @@ -1391,7 +1239,7 @@ Sensors::parameter_update_poll(bool forced) unsigned accel_count = 0; /* run through all gyro sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); @@ -1405,7 +1253,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1457,7 +1305,7 @@ Sensors::parameter_update_poll(bool forced) } /* run through all accel sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); @@ -1471,7 +1319,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1523,7 +1371,7 @@ Sensors::parameter_update_poll(bool forced) } /* run through all mag sensors */ - for (unsigned s = 0; s < 3; s++) { + for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { /* set a valid default rotation (same as board). * if the mag is configured, this might be replaced @@ -1544,7 +1392,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -2099,6 +1947,26 @@ Sensors::task_main_trampoline(int argc, char *argv[]) sensors::g_sensors->task_main(); } +int +Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, + unsigned *priorities, unsigned *errcount) +{ + unsigned group_count = orb_group_count(meta); + + if (group_count > SENSOR_COUNT_MAX) { + group_count = SENSOR_COUNT_MAX; + } + + for (unsigned i = 0; i < group_count; i++) { + if (subs[i] < 0) { + subs[i] = orb_subscribe_multi(meta, i); + orb_priority(subs[i], (int32_t*)&priorities[i]); + } + } + + return group_count; +} + void Sensors::task_main() { @@ -2129,74 +1997,47 @@ Sensors::task_main() return; } + struct sensor_combined_s raw = {}; + + /* ensure no overflows can occur */ + static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + /* * do subscriptions */ - _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); - _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); - _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); - _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); - _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); - _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); - _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); + + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); - _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* - * get sensor priorities - */ - orb_priority(_gyro_sub, &_gyro_prio); - orb_priority(_accel_sub, &_accel_prio); - orb_priority(_mag_sub, &_mag_prio); - orb_priority(_gyro1_sub, &_gyro1_prio); - orb_priority(_accel1_sub, &_accel1_prio); - orb_priority(_mag1_sub, &_mag1_prio); - orb_priority(_gyro2_sub, &_gyro2_prio); - orb_priority(_accel2_sub, &_accel2_prio); - orb_priority(_mag2_sub, &_mag2_prio); - orb_priority(_baro_sub, &_baro_prio); - orb_priority(_baro1_sub, &_baro1_prio); - /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; - /* set high initial error counts to deselect gyros */ - raw.gyro_errcount = 100000; - raw.gyro1_errcount = 100000; - raw.gyro2_errcount = 100000; - - /* set high initial error counts to deselect accels */ - raw.accelerometer_errcount = 100000; - raw.accelerometer1_errcount = 100000; - raw.accelerometer2_errcount = 100000; - - /* set high initial error counts to deselect mags */ - raw.magnetometer_errcount = 100000; - raw.magnetometer1_errcount = 100000; - raw.magnetometer2_errcount = 100000; - memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; @@ -2219,8 +2060,8 @@ Sensors::task_main() /* wakeup source(s) */ px4_pollfd_struct_t fds[1]; - /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ - fds[0].fd = _gyro_sub; + /* use the gyro to pace output */ + fds[0].fd = _gyro_sub[0]; fds[0].events = POLLIN; _task_should_exit = false; @@ -2245,6 +2086,21 @@ Sensors::task_main() /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); + /* keep adding sensors as long as we are not armed */ + if (!_armed) { + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], + &raw.gyro_priority[0], &raw.gyro_errcount[0]); + + _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + + _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + + _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], + &raw.baro_priority[0], &raw.baro_errcount[0]); + } + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); @@ -2253,13 +2109,13 @@ Sensors::task_main() baro_poll(raw); /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { - fds[0].fd = _gyro2_sub; + if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { + fds[0].fd = _gyro_sub[2]; } else { - fds[0].fd = _gyro1_sub; + fds[0].fd = _gyro_sub[1]; } }