diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ce267130ce..0273396b43 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -99,6 +99,7 @@ #include #include #include +#include #include @@ -249,6 +250,7 @@ private: orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ orb_advert_t _mavlink_log_pub; + orb_advert_t _sensor_preflight; /**< sensor preflight topic */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -273,6 +275,9 @@ private: uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */ + float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */ + hrt_abstime _vibration_warning_timestamp; bool _vibration_warning; @@ -548,6 +553,16 @@ private: */ bool check_vibration(); + /** + * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor + */ + void calc_accel_inconsistency(sensor_preflight_s &preflt); + + /** + * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor + */ + void calc_gyro_inconsistency(struct sensor_preflight_s &preflt); + /** * Shim for calling task_main from task_create. */ @@ -589,6 +604,7 @@ Sensors::Sensors() : _airspeed_pub(nullptr), _diff_pres_pub(nullptr), _mavlink_log_pub(nullptr), + _sensor_preflight(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), @@ -613,6 +629,8 @@ Sensors::Sensors() : memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); memset(&_last_mag_timestamp, 0, sizeof(_last_mag_timestamp)); memset(&_last_baro_timestamp, 0, sizeof(_last_baro_timestamp)); + memset(&_accel_diff, 0, sizeof(_accel_diff)); + memset(&_gyro_diff, 0, sizeof(_gyro_diff)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -2248,6 +2266,108 @@ Sensors::check_vibration() return ret; } +void +Sensors::calc_accel_inconsistency(sensor_preflight_s &preflt) +{ + // skip check if less than 2 sensors + if (_accel.subscription_count < 2) { + preflt.accel_inconsistency_m_s_s = 0.0f; + return; + + } + + float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared + uint8_t check_index = 0; // the number of sensors the primary has been checked against + + // Check each sensor against the primary + for (unsigned sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) { + + // check that the sensor we are checking against is not the same as the primary + if (sensor_index != _accel.last_best_vote) { + + float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary + + // calculate accel_diff_sum_sq for the specified sensor against the primary + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _accel_diff[axis_index][check_index] = 0.95f * _accel_diff[axis_index][check_index] + 0.05f * + (_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[axis_index] - + _last_sensor_data[sensor_index].accelerometer_m_s2[axis_index]); + accel_diff_sum_sq += _accel_diff[axis_index][check_index] * _accel_diff[axis_index][check_index]; + + } + + // capture the largest sum value + if (accel_diff_sum_sq > accel_diff_sum_max_sq) { + accel_diff_sum_max_sq = accel_diff_sum_sq; + + } + + // increment the check index + check_index++; + } + + // check to see if the maximum number of checks has been reached and break + if (check_index >= 2) { + break; + + } + } + + // get the vector length of the largest difference and write to the combined sensor struct + preflt.accel_inconsistency_m_s_s = sqrtf(accel_diff_sum_max_sq); +} + +void Sensors::calc_gyro_inconsistency(sensor_preflight_s &preflt) +{ + // skip check if less than 2 sensors + if (_gyro.subscription_count < 2) { + preflt.gyro_inconsistency_rad_s = 0.0f; + return; + + } + + float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared + uint8_t check_index = 0; // the number of sensors the primary has been checked against + + + // Check each sensor against the primary + for (unsigned sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) { + + // check that the sensor we are checking against is not the same as the primary + if (sensor_index != _gyro.last_best_vote) { + + float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary + + // calculate gyro_diff_sum_sq for the specified sensor against the primary + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _gyro_diff[axis_index][check_index] = 0.95f * _gyro_diff[axis_index][check_index] + 0.05f * + (_last_sensor_data[_gyro.last_best_vote].gyro_rad[axis_index] - + _last_sensor_data[sensor_index].gyro_rad[axis_index]); + gyro_diff_sum_sq += _gyro_diff[axis_index][check_index] * _gyro_diff[axis_index][check_index]; + + } + + // capture the largest sum value + if (gyro_diff_sum_sq > gyro_diff_sum_max_sq) { + gyro_diff_sum_max_sq = gyro_diff_sum_sq; + + } + + // increment the check index + check_index++; + } + + // check to see if the maximum number of checks has been reached and break + if (check_index >= 2) { + break; + + } + } + + // get the vector length of the largest difference and write to the combined sensor struct + preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq); +} + void Sensors::task_main_trampoline(int argc, char *argv[]) { @@ -2310,6 +2430,8 @@ Sensors::task_main() raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + struct sensor_preflight_s preflt = {}; + /* * do subscriptions */ @@ -2360,6 +2482,13 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + /* advertise the sensor_preflight topic and make the initial publication */ + preflt.accel_inconsistency_m_s_s = 0.0f; + + preflt.gyro_inconsistency_rad_s = 0.0f; + + _sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt); + /* wakeup source */ px4_pollfd_struct_t poll_fds = {}; @@ -2406,7 +2535,6 @@ Sensors::task_main() mag_poll(raw); baro_poll(raw); - /* check battery voltage */ adc_poll(raw); @@ -2434,6 +2562,16 @@ Sensors::task_main() check_failover(_mag, "Mag"); check_failover(_baro, "Baro"); + /* If the the vehicle is disarmed calculate the length of the maximum difference between + * IMU units as a consistency metric and publish to the sensor preflight topic + */ + if (!_armed) { + calc_accel_inconsistency(preflt); + calc_gyro_inconsistency(preflt); + orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt); + + } + //check_vibration(); //disabled for now, as it does not seem to be reliable }