diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 790cdf3c68..8a7045f9d7 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -5,18 +5,18 @@ # change with board revisions and sensor updates. # -uint64[3] gyro_timestamp # Gyro timestamps -float32[9] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame -uint64[3] gyro_integral_dt # delta time for gyro integral in us +# gyro timstamp is equal to the timestamp of the message +float32[3] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame +uint64 gyro_integral_dt # delta time for gyro integral in us -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 -uint64[3] accelerometer_timestamp # Accelerometer timestamp +uint64 accelerometer_timestamp # Accelerometer timestamp +float32[3] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 +uint64 accelerometer_integral_dt # delta time for accel integral in us -float32[9] magnetometer_ga # Magnetic field in NED body frame, in Gauss -uint64[3] magnetometer_timestamp # Magnetometer timestamp +uint64 magnetometer_timestamp # Magnetometer timestamp +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss -float32[3] baro_alt_meter # Altitude, already temp. comp. -float32[3] baro_temp_celcius # Temperature in degrees celsius -uint64[3] baro_timestamp # Barometer timestamp +uint64 baro_timestamp # Barometer timestamp +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 246ad91d42..47330774a3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -205,7 +205,7 @@ void frsky_send_frame1(int uart) { /* send formatted frame */ float acceleration[3]; - float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f; acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt; acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt; acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt; @@ -214,12 +214,12 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - sensor_combined->baro_alt_meter[0]); + sensor_combined->baro_alt_meter); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f)); + roundf(frac(sensor_combined->baro_alt_meter) * 100.0f)); frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(sensor_combined->baro_temp_celcius[0])); + roundf(sensor_combined->baro_temp_celcius)); frsky_send_data(uart, FRSKY_ID_VFAS, roundf(battery_status->voltage_v * 10.0f)); diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index c266b84924..c728ea6f8a 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -236,7 +236,7 @@ void sPort_send_CUR(int uart) void sPort_send_ALT(int uart) { /* send data */ - uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]); + uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 1150fd0722..92b1664ccf 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -149,12 +149,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[0] + 20); + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 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[0] + 500); + uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index a382c2224a..58ec077170 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -102,7 +102,7 @@ int px4_simple_app_main(int argc, char *argv[]) /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); float sensor_accel[3]; - float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = raw.accelerometer_integral_dt / 1.e6f; sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt; sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt; sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt; diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index d8c88e618d..d42f57b27e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -68,7 +68,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); matrix::Vector a; - float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = sensor->accelerometer_integral_dt / 1.e6f; a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt; a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt; a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt; 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 fa22ad165f..1a66b19f94 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -396,7 +396,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } float gyro_rad_s[3]; - float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f; + float gyro_dt = raw.gyro_integral_dt / 1.e6f; gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt; gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt; gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt; @@ -427,10 +427,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.gyro_timestamp[0]) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.gyro_timestamp[0]; + sensor_last_timestamp[0] = raw.timestamp; } z_k[0] = gyro_rad_s[0] - gyro_offsets[0]; @@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[2] = gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.accelerometer_timestamp[0]; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; @@ -477,7 +477,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } matrix::Vector3f raw_accel; - float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = raw.accelerometer_integral_dt / 1.e6f; raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt; raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt; raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt; @@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) z_k[5] = raw_accel(2) - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] && + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && /* 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[0]; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } bool vision_updated = false; 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 2a6a743661..f56a6793aa 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -139,7 +138,6 @@ private: param_t mag_decl_auto; param_t acc_comp; param_t bias_max; - param_t vibe_thresh; param_t ext_hdg_mode; param_t airspeed_mode; } _params_handles; /**< handles for interesting parameters */ @@ -152,8 +150,6 @@ private: bool _mag_decl_auto = false; bool _acc_comp = false; float _bias_max = 0.0f; - float _vibration_warning_threshold = 2.0f; - hrt_abstime _vibration_warning_timestamp = 0; int _ext_hdg_mode = 0; int _airspeed_mode = 0; @@ -177,10 +173,6 @@ private: 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; @@ -190,8 +182,6 @@ private: bool _inited = false; bool _data_good = false; - bool _failsafe = false; - bool _vibration_warning = false; bool _ext_hdg_good = false; orb_advert_t _mavlink_log_pub = nullptr; @@ -215,15 +205,10 @@ private: AttitudeEstimatorQ::AttitudeEstimatorQ() : _vel_prev(0, 0, 0), _pos_acc(0, 0, 0), - _voter_gyro(3), - _voter_accel(3), - _voter_mag(3), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_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_ext_hdg = param_find("ATT_W_EXT_HDG"); @@ -232,7 +217,6 @@ 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"); _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); _params_handles.airspeed_mode = param_find("FW_ARSP_MODE"); } @@ -286,12 +270,6 @@ int AttitudeEstimatorQ::start() 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[]) @@ -346,129 +324,40 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; - int best_gyro = 0; - int best_accel = 0; - int best_mag = 0; - if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data - for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) { + if (sensors.timestamp > 0) { + float gyro_dt = sensors.gyro_integral_dt / 1e6; + _gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt; + _gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt; + _gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt; + } - /* ignore empty fields */ - if (sensors.gyro_timestamp[i] > 0) { + if (sensors.accelerometer_timestamp > 0) { + float accel_dt = sensors.accelerometer_integral_dt / 1.e6f; + _accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt; + _accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt; + _accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt; - float gyro[3]; - float gyro_dt = sensors.gyro_integral_dt[i] / 1e6; - gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt; - gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt; - gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt; - - //TODO: note: voter will be moved into sensors module - //_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); - _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75); - } - - if (sensors.accelerometer_timestamp[i] > 0) { - float acceleration[3]; - float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f; - acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; - acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; - acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; - _voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75); - } - - if (sensors.magnetometer_timestamp[i] > 0) { - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75); + if (_accel.length() < 0.01f) { + warnx("WARNING: degenerate accel!"); + continue; } } - // 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 (sensors.magnetometer_timestamp > 0) { + _mag(0) = sensors.magnetometer_ga[0]; + _mag(1) = sensors.magnetometer_ga[1]; + _mag(2) = sensors.magnetometer_ga[2]; - if (_accel.length() < 0.01f) { - warnx("WARNING: degenerate accel!"); - continue; - } - - if (_mag.length() < 0.01f) { - warnx("WARNING: degenerate mag!"); - continue; + if (_mag.length() < 0.01f) { + warnx("WARNING: degenerate mag!"); + continue; + } } _data_good = true; - - if (!_failsafe) { - uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; - -#ifdef __PX4_POSIX - perf_end(_perf_accel); - perf_end(_perf_mpu); - perf_end(_perf_mag); -#endif - - if (_voter_gyro.failover_count() > 0) { - _failsafe = true; - flags = _voter_gyro.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!", - _voter_gyro.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_voter_accel.failover_count() > 0) { - _failsafe = true; - flags = _voter_accel.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!", - _voter_accel.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_voter_mag.failover_count() > 0) { - _failsafe = true; - flags = _voter_mag.failover_state(); - mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!", - _voter_mag.failover_index(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); - } - - if (_failsafe) { - mavlink_and_console_log_emergency(&_mavlink_log_pub, "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_log_pub, "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; - } } // Update vision and motion capture heading @@ -596,10 +485,6 @@ void AttitudeEstimatorQ::task_main() memcpy(&att.q[0], _q.data, sizeof(att.q)); att.q_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()); - /* the instance count is not used here */ int att_inst; orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); @@ -656,12 +541,9 @@ void AttitudeEstimatorQ::task_main() } { - struct estimator_status_s est = {}; + //struct estimator_status_s est = {}; - est.timestamp = sensors.timestamp; - est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0); - est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1); - est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2); + //est.timestamp = sensors.timestamp; /* the instance count is not used here */ //int est_inst; @@ -672,6 +554,12 @@ void AttitudeEstimatorQ::task_main() } } +#ifdef __PX4_POSIX + perf_end(_perf_accel); + perf_end(_perf_mpu); + perf_end(_perf_mag); +#endif + orb_unsubscribe(_sensors_sub); orb_unsubscribe(_vision_sub); orb_unsubscribe(_mocap_sub); @@ -706,7 +594,6 @@ 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); param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); param_get(_params_handles.airspeed_mode, &_airspeed_mode); } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 39dc65155e..7138f34309 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, for (unsigned i = 0; i < ndim; i++) { - float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f); + float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f); float d = di - accel_ema[i]; accel_ema[i] += d * w; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 81ccc4e852..c55c2d57d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1788,7 +1788,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[0]) < FAILSAFE_DEFAULT_TIMEOUT) { + if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ if (status_flags.barometer_failure) { status_flags.barometer_failure = false; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ddc471e234..cb6e21d3aa 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -505,14 +505,14 @@ void Ekf2::task_main() } // push imu data into estimator - _ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], - &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); + _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, + sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s); // read mag data - _ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); + _ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga); // read baro data - _ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); + _ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter); // read gps data if available if (gps_updated) { @@ -615,7 +615,7 @@ void Ekf2::task_main() float gyro_bias[3] = {}; _ekf.get_gyro_bias(gyro_bias); float gyro_rad_s[3]; - float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f; + float gyro_dt = sensors.gyro_integral_dt / 1.e6f; gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0]; gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1]; gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2]; @@ -650,7 +650,7 @@ void Ekf2::task_main() // Acceleration data matrix::Vector acceleration; - float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = sensors.accelerometer_integral_dt / 1.e6f; acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt; acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt; acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt; @@ -811,7 +811,7 @@ void Ekf2::task_main() // TODO use innovatun consistency check timouts to set this global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning - global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) if (_vehicle_global_position_pub == nullptr) { _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); @@ -905,15 +905,15 @@ void Ekf2::task_main() if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.time_ref = now; - replay.gyro_integral_dt = sensors.gyro_integral_dt[0]; - replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0]; - replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0]; - replay.baro_timestamp = sensors.baro_timestamp[0]; - memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad)); - memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], + replay.gyro_integral_dt = sensors.gyro_integral_dt; + replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt; + replay.magnetometer_timestamp = sensors.magnetometer_timestamp; + replay.baro_timestamp = sensors.baro_timestamp; + memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad)); + memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s, sizeof(replay.accelerometer_integral_m_s)); - memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga)); - replay.baro_alt_meter = sensors.baro_alt_meter[0]; + memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga)); + replay.baro_alt_meter = sensors.baro_alt_meter; // only write gps data if we had a gps update. if (gps_updated) { diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 7faae2f89f..b92014081f 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -377,10 +377,10 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref; parseMessage(data, dest_ptr, type); _sensors.timestamp = replay_part1.time_ref; - _sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt; - _sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt; - _sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp; - _sensors.baro_timestamp[0] = replay_part1.baro_timestamp; + _sensors.gyro_integral_dt = replay_part1.gyro_integral_dt; + _sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt; + _sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp; + _sensors.baro_timestamp = replay_part1.baro_timestamp; _sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad; _sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad; _sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad; @@ -390,7 +390,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) _sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga; _sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga; _sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga; - _sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter; + _sensors.baro_alt_meter = replay_part1.baro_alt_meter; _part1_counter_ref = _message_counter; } else if (type == LOG_RPL2_MSG) { diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index b66025c30b..92a729a413 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -71,7 +71,6 @@ #include #include #include -#include #include "estimator_22states.h" #include @@ -191,8 +190,6 @@ 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 = 2.0f; - hrt_abstime _vibration_warning_timestamp = 0; perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _loop_intvl; ///< loop rate counter @@ -213,15 +210,7 @@ private: hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; hrt_abstime _distance_last_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 bool _was_landed; ///< indicates if was landed in previous iteration 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 ac19b192fb..d426c8d3ea 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 @@ -205,15 +205,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _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), _was_landed(true), @@ -236,8 +228,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _LP_att_Q(250.0f, 20.0f), _LP_att_R(250.0f, 20.0f) { - _voter_mag.set_timeout(200000); - _terrain_estimator = new TerrainEstimator(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); @@ -620,19 +610,9 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); - /* HIL is slow, set permissive timeouts */ - _voter_gyro.set_timeout(500000); - _voter_accel.set_timeout(500000); - _voter_mag.set_timeout(500000); - /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - /* set sensors to de-initialized state */ - _gyro_main = -1; - _accel_main = -1; - _mag_main = -1; - _baro_init = false; _gps_initialized = false; @@ -664,7 +644,8 @@ void AttitudePositionEstimatorEKF::task_main() * We run the filter only once all data has been fetched **/ - if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) { + if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp + && _sensor_combined.magnetometer_timestamp) { // maintain filtered baro and gps altitudes to calculate weather offset // baro sample rate is ~70Hz and measurement bandwidth is high @@ -1343,13 +1324,6 @@ 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() @@ -1382,97 +1356,37 @@ void AttitudePositionEstimatorEKF::pollData() /* fill in last data set */ _ekf->dtIMU = deltaT; - // Feed validator with recent sensor data + _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0]; + _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1]; + _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2]; - //TODO: note, we will move voters into sensors module - for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { - float gyro_rad_s[3]; - float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f; - gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt; - gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt; - gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt; - _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75); - float acceleration[3]; - float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f; - acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; - acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; - acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; - _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75); - _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75); - } + float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f; + _ekf->angRate = _ekf->dAngIMU / gyro_dt; - // Get best measurement values - hrt_abstime curr_time = hrt_absolute_time(); - (void)_voter_gyro.get_best(curr_time, &_gyro_main); + perf_count(_perf_gyro); - if (_gyro_main >= 0) { + if (_last_accel != _sensor_combined.accelerometer_timestamp) { - _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]; + _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0]; + _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1]; + _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2]; - float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f; - _ekf->angRate = _ekf->dAngIMU / gyro_dt; - - 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])) { - - _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]; - - float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f; + float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f; _ekf->accel = _ekf->dVelIMU / accel_dt; - _last_accel = _sensor_combined.accelerometer_timestamp[_accel_main]; + _last_accel = _sensor_combined.accelerometer_timestamp; } - (void)_voter_mag.get_best(curr_time, &_mag_main); + Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1], + _sensor_combined.magnetometer_ga[2]); - 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_log_pub, "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_log_pub, "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; + if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) { + _ekf->magData.x = mag.x; + _ekf->magData.y = mag.y; + _ekf->magData.z = mag.z; + _newDataMag = true; + _last_mag = _sensor_combined.magnetometer_timestamp; + perf_count(_perf_mag); } _last_sensor_timestamp = _sensor_combined.timestamp; @@ -1481,8 +1395,8 @@ void AttitudePositionEstimatorEKF::pollData() // leave this in as long as larger improvements are still being made. #if 0 - float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; - float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; + float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f; + float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f; static unsigned dtoverflow5 = 0; static unsigned dtoverflow10 = 0; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3fddf2c7b6..f604581a75 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1231,7 +1231,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* filter speed and altitude for controller */ math::Vector<3> accel_body; - float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f; accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt; accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt; accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 493a12e997..cb0301f8a0 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -744,7 +744,7 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().terrain_alt = _altHome - xLP(X_tz); _pub_gpos.get().terrain_alt_valid = _validTZ; _pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout; - _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0]; + _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; _pub_gpos.update(); } } @@ -845,7 +845,7 @@ void BlockLocalPositionEstimator::predict() if (integrate && _sub_att.get().R_valid) { Matrix3f R_att(_sub_att.get().R); Vector3f a; - float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f; a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt; a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt; a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt; diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index 57fc87eb5f..ffe8e51611 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -41,7 +41,7 @@ int BlockLocalPositionEstimator::baroMeasure(Vector &y) { //measure y.setZero(); - y(0) = _sub_sensor.get().baro_alt_meter[0]; + y(0) = _sub_sensor.get().baro_alt_meter; _baroStats.update(y); _time_last_baro = _timeStamp; return OK; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a699974ca1..da77fd07ba 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -724,39 +724,39 @@ protected: if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; - if (_accel_timestamp != sensor.accelerometer_timestamp[0]) { + if (_accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - _accel_timestamp = sensor.accelerometer_timestamp[0]; + _accel_timestamp = sensor.accelerometer_timestamp; } - if (_gyro_timestamp != sensor.gyro_timestamp[0]) { + if (_gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - _gyro_timestamp = sensor.gyro_timestamp[0]; + _gyro_timestamp = sensor.timestamp; } - if (_mag_timestamp != sensor.magnetometer_timestamp[0]) { + if (_mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - _mag_timestamp = sensor.magnetometer_timestamp[0]; + _mag_timestamp = sensor.magnetometer_timestamp; } - if (_baro_timestamp != sensor.baro_timestamp[0]) { + if (_baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - _baro_timestamp = sensor.baro_timestamp[0]; + _baro_timestamp = sensor.baro_timestamp; } _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); mavlink_highres_imu_t msg; msg.time_usec = sensor.timestamp; - float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = sensor.accelerometer_integral_dt / 1.e6f; msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt; msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt; msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt; - float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f; + float gyro_dt = sensor.gyro_integral_dt / 1.e6f; msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt; msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt; msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt; @@ -765,8 +765,8 @@ protected: msg.zmag = sensor.magnetometer_ga[2]; msg.abs_pressure = 0; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; - msg.pressure_alt = sensor.baro_alt_meter[0]; - msg.temperature = sensor.baro_temp_celcius[0]; + msg.pressure_alt = sensor.baro_alt_meter; + msg.temperature = sensor.baro_temp_celcius; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); @@ -1014,7 +1014,7 @@ protected: /* fall back to baro altitude */ sensor_combined_s sensor; (void)_sensor_sub->update(&_sensor_time, &sensor); - msg.alt = sensor.baro_alt_meter[0]; + msg.alt = sensor.baro_alt_meter; } msg.climb = -pos.vel_d; @@ -3121,7 +3121,7 @@ protected: msg.time_usec = hrt_absolute_time(); - msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN; + msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN; msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN; msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN; msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0d62fb2307..62cf3b3f9c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1673,16 +1673,14 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { struct sensor_combined_s hil_sensors = {}; - hil_sensors.timestamp = timestamp; - hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; _hil_prev_gyro[0] = imu.xgyro; _hil_prev_gyro[1] = imu.ygyro; _hil_prev_gyro[2] = imu.zgyro; - hil_sensors.gyro_integral_dt[0] = dt * 1e6f; - hil_sensors.gyro_timestamp[0] = timestamp; + hil_sensors.gyro_integral_dt = dt * 1e6f; + hil_sensors.timestamp = timestamp; hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; @@ -1690,17 +1688,17 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) _hil_prev_accel[0] = imu.xacc; _hil_prev_accel[1] = imu.yacc; _hil_prev_accel[2] = imu.zacc; - hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; - hil_sensors.accelerometer_timestamp[0] = timestamp; + hil_sensors.accelerometer_integral_dt = dt * 1e6f; + hil_sensors.accelerometer_timestamp = timestamp; hil_sensors.magnetometer_ga[0] = imu.xmag; hil_sensors.magnetometer_ga[1] = imu.ymag; hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_timestamp[0] = timestamp; + hil_sensors.magnetometer_timestamp = timestamp; - 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.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_timestamp = 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 163238da75..c3e606da0d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -500,7 +500,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 96f02298a1..7ef5cd7106 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -425,14 +425,14 @@ 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[0] != baro_timestamp) { - baro_timestamp = sensor.baro_timestamp[0]; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; baro_wait_for_sample_time = hrt_absolute_time(); /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { - baro_offset += sensor.baro_alt_meter[0]; + if (PX4_ISFINITE(sensor.baro_alt_meter)) { + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } @@ -502,10 +502,10 @@ 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[0] != accel_timestamp) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { float sensor_accel[3]; - float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f; + float accel_dt = sensor.accelerometer_integral_dt / 1.e6f; sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0]; sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1]; sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2]; @@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(acc, 0, sizeof(acc)); } - accel_timestamp = sensor.accelerometer_timestamp[0]; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - 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]; + if (sensor.baro_timestamp != baro_timestamp) { + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } @@ -1363,7 +1363,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.terrain_alt_valid = false; } - global_pos.pressure_alt = sensor.baro_alt_meter[0]; + global_pos.pressure_alt = sensor.baro_alt_meter; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9d0245dcaf..3ad1925c6f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1370,10 +1370,10 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - 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 gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; /* initialize calculated mean SNR */ float snr_mean = 0.0f; @@ -1634,80 +1634,58 @@ int sdlog2_thread_main(int argc, char *argv[]) // but we need to copy it again because we are re-using the buffer. orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - for (unsigned i = 0; i < 3; i++) { - bool write_IMU = false; - bool write_SENS = false; + 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.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; + 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.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; + 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.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; + 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.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; + 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; - } + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; - float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt; - float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt; - 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 = 0; - log_msg.body.log_IMU.temp_acc = 0; - log_msg.body.log_IMU.temp_mag = 0; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } + float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt; + float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt; + 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 = 0; + log_msg.body.log_IMU.temp_acc = 0; + log_msg.body.log_IMU.temp_mag = 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; - } + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = 0; - 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 = 0; - log_msg.body.log_SENS.diff_pres_filtered = 0; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } + log_msg.body.log_SENS.baro_pres = 0; + 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 = 0; + log_msg.body.log_SENS.diff_pres_filtered = 0; + LOGBUFFER_WRITE_AND_COUNT(SENS); } /* --- VTOL VEHICLE STATUS --- */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e03ee4f3a1..658998d879 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -76,6 +76,7 @@ #include #include +#include #include #include #include @@ -85,6 +86,7 @@ #include #include +#include #include #include @@ -164,6 +166,9 @@ public: */ int start(); + + void print_status(); + private: static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ @@ -200,19 +205,33 @@ private: int _sensors_task; /**< task handle for sensor task */ bool _hil_enabled; /**< if true, HIL is active */ - bool _publishing; /**< if true, we are publishing sensor data */ + bool _publishing; /**< if true, we are publishing sensor data (in HIL mode, we don't) */ bool _armed; /**< arming status of the vehicle */ - 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 */ - int _actuator_ctrl_0_sub; /**< attitude controls sub */ - 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 */ + struct SensorData { + SensorData() + : subscription_count(0), + voter(SENSOR_COUNT_MAX), + last_failover_count(0) + { + for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + subscription[i] = -1; + } + } + int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ + uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */ + int subscription_count; + DataValidatorGroup voter; + unsigned int last_failover_count; + }; + + SensorData _gyro; + SensorData _accel; + SensorData _mag; + SensorData _baro; + + int _actuator_ctrl_0_sub; /**< attitude controls sub */ int _rc_sub; /**< raw rc channels data subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ @@ -227,6 +246,7 @@ private: orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ + orb_advert_t _mavlink_log_pub; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -234,7 +254,6 @@ private: struct rc_channels_s _rc; /**< r/c channel data */ struct battery_status_s _battery_status; /**< battery status */ - struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; @@ -245,9 +264,12 @@ private: Battery _battery; /**< Helper lib to publish battery_status topic. */ - float _latest_baro_pressure[SENSOR_COUNT_MAX]; - hrt_abstime _last_accel_timestamp[SENSOR_COUNT_MAX]; - hrt_abstime _last_gyro_timestamp[SENSOR_COUNT_MAX]; + float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */ + float _last_best_baro_pressure; /**< pressure from last best baro */ + sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + + hrt_abstime _vibration_warning_timestamp; + bool _vibration_warning; struct { float min[_rc_max_chan_count]; @@ -320,6 +342,8 @@ private: float baro_qnh; + float vibration_warning_threshold; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -387,10 +411,12 @@ private: param_t baro_qnh; + param_t vibe_thresh; /**< vibration threshold */ + } _parameter_handles; /**< handles for interesting parameters */ - int init_sensor_class(const struct orb_metadata *meta, int *subs); + void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data); /** * Update our local parameter cache. @@ -495,6 +521,18 @@ private: */ void adc_poll(struct sensor_combined_s &raw); + /** + * Check & handle failover of a sensor + * @return true if a switch occured (could be for a non-critical reason) + */ + bool check_failover(SensorData &sensor, const char *sensor_name); + + /** + * check vibration levels and output a warning if they're high + * @return true on high vibration + */ + bool check_vibration(); + /** * Shim for calling task_main from task_create. */ @@ -521,10 +559,6 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), _armed(false), - _gyro_count(0), - _accel_count(0), - _mag_count(0), - _baro_count(0), _rc_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), @@ -539,6 +573,7 @@ Sensors::Sensors() : _battery_pub(nullptr), _airspeed_pub(nullptr), _diff_pres_pub(nullptr), + _mavlink_log_pub(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), @@ -546,23 +581,20 @@ Sensors::Sensors() : _param_rc_values{}, _board_rotation{}, - _mag_rotation{} + _mag_rotation{}, + + _last_best_baro_pressure(0.f), + + _vibration_warning_timestamp(0), + _vibration_warning(false) { - /* 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; - } + _mag.voter.set_timeout(200000); memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); - memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure)); - memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); - memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp)); + memset(&_last_sensor_data, 0, sizeof(_last_sensor_data)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -661,6 +693,8 @@ Sensors::Sensors() : /* Barometer QNH */ _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + _parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + // These are parameters for which QGroundControl always expects to be returned in a list request. // We do a param_find here to force them into the list. (void)param_find("RC_CHAN_CNT"); @@ -993,6 +1027,8 @@ Sensors::parameters_update() #endif + param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold); + return ret; } @@ -1014,43 +1050,72 @@ Sensors::adc_init() void Sensors::accel_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _accel_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _accel.subscription_count; i++) { bool accel_updated; - orb_check(_accel_sub[i], &accel_updated); + orb_check(_accel.subscription[i], &accel_updated); if (accel_updated) { - struct accel_report accel_report; + struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report); + + if (accel_report.timestamp == 0) { + continue; //ignore invalid data + } + + got_update = true; + math::Vector<3> sensor_value; if (accel_report.integral_dt != 0) { 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_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); + _last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0); + _last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1); + _last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2); - raw.accelerometer_integral_dt[i] = accel_report.integral_dt; + _last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt; + + float dt = accel_report.integral_dt / 1.e6f; + sensor_value = vect_int / dt; } else { //this is undesirable: a driver did not set the integral, so we have to construct it ourselves math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z); vect_val = _board_rotation * vect_val; - if (_last_accel_timestamp[i] == 0) { - _last_accel_timestamp[i] = accel_report.timestamp - 1000; + sensor_value = vect_val; + + if (_last_sensor_data[i].accelerometer_timestamp == 0) { + _last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000; } - raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i]; - _last_accel_timestamp[i] = accel_report.timestamp; - float dt = raw.accelerometer_integral_dt[i] / 1.e6f; - raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt; - raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt; - raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt; + _last_sensor_data[i].accelerometer_integral_dt = + accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp; + float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f; + _last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt; + _last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt; + _last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt; } - raw.accelerometer_timestamp[i] = accel_report.timestamp; + _last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp; + _accel.voter.put(i, accel_report.timestamp, sensor_value.data, + accel_report.error_count, _accel.priority[i]); + } + } + + if (got_update) { + int best_index; + _accel.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0]; + raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1]; + raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2]; + raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt; + raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp; } } } @@ -1058,47 +1123,72 @@ Sensors::accel_poll(struct sensor_combined_s &raw) void Sensors::gyro_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _gyro_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _gyro.subscription_count; i++) { bool gyro_updated; - orb_check(_gyro_sub[i], &gyro_updated); + orb_check(_gyro.subscription[i], &gyro_updated); if (gyro_updated) { - struct gyro_report gyro_report; + struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report); + + if (gyro_report.timestamp == 0) { + continue; //ignore invalid data + } + + got_update = true; + math::Vector<3> sensor_value; if (gyro_report.integral_dt != 0) { math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); vect_int = _board_rotation * vect_int; - 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); + _last_sensor_data[i].gyro_integral_rad[0] = vect_int(0); + _last_sensor_data[i].gyro_integral_rad[1] = vect_int(1); + _last_sensor_data[i].gyro_integral_rad[2] = vect_int(2); - raw.gyro_integral_dt[i] = gyro_report.integral_dt; - raw.gyro_timestamp[i] = gyro_report.timestamp; + _last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt; + + float dt = gyro_report.integral_dt / 1.e6f; + sensor_value = vect_int / dt; } else { //this is undesirable: a driver did not set the integral, so we have to construct it ourselves math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z); vect_val = _board_rotation * vect_val; - if (_last_gyro_timestamp[i] == 0) { - _last_gyro_timestamp[i] = gyro_report.timestamp - 1000; + sensor_value = vect_val; + + if (_last_sensor_data[i].timestamp == 0) { + _last_sensor_data[i].timestamp = gyro_report.timestamp - 1000; } - raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i]; - _last_gyro_timestamp[i] = gyro_report.timestamp; - float dt = raw.gyro_integral_dt[i] / 1.e6f; - raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt; - raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt; - raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt; - + _last_sensor_data[i].gyro_integral_dt = + gyro_report.timestamp - _last_sensor_data[i].timestamp; + float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f; + _last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt; + _last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt; + _last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt; } - if (i == 0) { - raw.timestamp = gyro_report.timestamp; - } + _last_sensor_data[i].timestamp = gyro_report.timestamp; + _gyro.voter.put(i, gyro_report.timestamp, sensor_value.data, + gyro_report.error_count, _gyro.priority[i]); + } + } + + if (got_update) { + int best_index; + _gyro.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0]; + raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1]; + raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2]; + raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt; + raw.timestamp = _last_sensor_data[best_index].timestamp; } } } @@ -1106,24 +1196,44 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) void Sensors::mag_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _mag_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _mag.subscription_count; i++) { bool mag_updated; - orb_check(_mag_sub[i], &mag_updated); + orb_check(_mag.subscription[i], &mag_updated); if (mag_updated) { - struct mag_report mag_report; + struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag.subscription[i], &mag_report); + if (mag_report.timestamp == 0) { + continue; //ignore invalid data + } + + got_update = true; math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - vect = _mag_rotation[i] * vect; - raw.magnetometer_ga[i * 3 + 0] = vect(0); - raw.magnetometer_ga[i * 3 + 1] = vect(1); - raw.magnetometer_ga[i * 3 + 2] = vect(2); + _last_sensor_data[i].magnetometer_ga[0] = vect(0); + _last_sensor_data[i].magnetometer_ga[1] = vect(1); + _last_sensor_data[i].magnetometer_ga[2] = vect(2); - raw.magnetometer_timestamp[i] = mag_report.timestamp; + _last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp; + _mag.voter.put(i, mag_report.timestamp, vect.data, + mag_report.error_count, _mag.priority[i]); + } + } + + if (got_update) { + int best_index; + _mag.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0]; + raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1]; + raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2]; + raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp; } } } @@ -1131,19 +1241,43 @@ Sensors::mag_poll(struct sensor_combined_s &raw) void Sensors::baro_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _baro_count; i++) { + bool got_update = false; + + for (unsigned i = 0; i < _baro.subscription_count; i++) { bool baro_updated; - orb_check(_baro_sub[i], &baro_updated); + orb_check(_baro.subscription[i], &baro_updated); if (baro_updated) { + struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report); - _latest_baro_pressure[i] = _barometer.pressure; - raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters - raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius + if (baro_report.timestamp == 0) { + continue; //ignore invalid data + } - raw.baro_timestamp[i] = _barometer.timestamp; + got_update = true; + math::Vector<3> vect(baro_report.altitude, 0.f, 0.f); + + _last_sensor_data[i].baro_alt_meter = baro_report.altitude; + _last_sensor_data[i].baro_temp_celcius = baro_report.temperature; + _last_baro_pressure[i] = baro_report.pressure; + + _last_sensor_data[i].baro_timestamp = baro_report.timestamp; + _baro.voter.put(i, baro_report.timestamp, vect.data, + baro_report.error_count, _baro.priority[i]); + } + } + + if (got_update) { + int best_index; + _baro.voter.get_best(hrt_absolute_time(), &best_index); + + if (best_index >= 0) { + raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter; + raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius; + raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp; + _last_best_baro_pressure = _last_baro_pressure[best_index]; } } } @@ -1158,7 +1292,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : - (raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG); + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; @@ -1175,13 +1309,12 @@ 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)); - //FIXME: we just use the baro pressure from the first baro. we should do voting instead. _airspeed.true_airspeed_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f, - _latest_baro_pressure[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _last_best_baro_pressure * 1e2f, + _last_best_baro_pressure * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f, - _latest_baro_pressure[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _last_best_baro_pressure * 1e2f, + _last_best_baro_pressure * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1544,14 +1677,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -1565,14 +1691,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -1586,14 +1705,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2) /* On most systems, we can just use the IOCTL call to set the calibration params. */ - const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); - - if (res) { - return false; - - } else { - return true; - } + return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); #else /* On QURT, the params are read directly in the respective wrappers. */ @@ -2045,14 +2157,72 @@ Sensors::rc_poll() } } +bool +Sensors::check_failover(SensorData &sensor, const char *sensor_name) +{ + if (sensor.last_failover_count != sensor.voter.failover_count()) { + + uint32_t flags = sensor.voter.failover_state(); + + if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { + //we switched due to a non-critical reason. No need to panic. + PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index()); + + } else { + mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failure :%s%s%s%s%s!", + sensor_name, + sensor.voter.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); + } + + sensor.last_failover_count = sensor.voter.failover_count(); + return true; + } + + return false; +} + +bool +Sensors::check_vibration() +{ + bool ret = false; + hrt_abstime cur_time = hrt_absolute_time(); + + if (!_vibration_warning && (_gyro.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold || + _accel.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold || + _mag.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold)) { + + if (_vibration_warning_timestamp == 0) { + _vibration_warning_timestamp = cur_time; + + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000 * 1000) { + _vibration_warning = true; + mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d", + (int)(100 * _gyro.voter.get_vibration_factor(cur_time)), + (int)(100 * _accel.voter.get_vibration_factor(cur_time)), + (int)(100 * _mag.voter.get_vibration_factor(cur_time))); + ret = true; + } + + } else { + _vibration_warning_timestamp = 0; + } + + return ret; +} + void 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) +void +Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data) { unsigned group_count = orb_group_count(meta); @@ -2061,12 +2231,19 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs) } for (unsigned i = 0; i < group_count; i++) { - if (subs[i] < 0) { - subs[i] = orb_subscribe_multi(meta, i); + if (sensor_data.subscription[i] < 0) { + sensor_data.subscription[i] = orb_subscribe_multi(meta, i); + int32_t priority; + orb_priority(sensor_data.subscription[i], &priority); + sensor_data.priority[i] = (uint8_t)priority; } + + int32_t priority; + orb_priority(sensor_data.subscription[i], &priority); + sensor_data.priority[i] = (uint8_t)priority; } - return group_count; + sensor_data.subscription_count = group_count; } void @@ -2090,84 +2267,79 @@ Sensors::task_main() 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 */ + init_sensor_class(ORB_ID(sensor_gyro), _gyro); - unsigned gcount_prev = _gyro_count; + init_sensor_class(ORB_ID(sensor_mag), _mag); - unsigned mcount_prev = _mag_count; + init_sensor_class(ORB_ID(sensor_accel), _accel); - unsigned acount_prev = _accel_count; + init_sensor_class(ORB_ID(sensor_baro), _baro); - unsigned bcount_prev = _baro_count; - - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); - - _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub); - - _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub); - - _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub); - - if (gcount_prev != _gyro_count || - mcount_prev != _mag_count || - acount_prev != _accel_count || - bcount_prev != _baro_count) { - - /* reload calibration params */ - parameter_update_poll(true); - } + /* reload calibration params */ + parameter_update_poll(true); _rc_sub = orb_subscribe(ORB_ID(input_rc)); + _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)); + _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); - /* - * do advertisements - */ - raw.timestamp = hrt_absolute_time(); + raw.timestamp = 0; _battery.reset(&_battery_status); /* get a set of initial values */ accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + diff_pres_poll(raw); parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ - px4_pollfd_struct_t fds[1] = {}; + px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {}; - /* use the gyro to pace output */ - fds[0].fd = _gyro_sub[0]; - fds[0].events = POLLIN; + int num_poll_fds = 0; _task_should_exit = false; - raw.timestamp = 0; - uint64_t _last_config_update = hrt_absolute_time(); while (!_task_should_exit) { + /* use the gyro(s) to pace output */ + if (num_poll_fds != _gyro.subscription_count) { //happens the first time we enter, or when new gyro added + num_poll_fds = _gyro.subscription_count; + + for (int i = 0; i < _gyro.subscription_count; ++i) { + fds[i].fd = _gyro.subscription[i]; + fds[i].events = POLLIN; + } + } + /* wait for up to 50ms for data */ - int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 50); + int pret = px4_poll(fds, num_poll_fds, 50); /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ @@ -2176,11 +2348,12 @@ Sensors::task_main() /* if the polling operation failed because no gyro sensor is available yet, * then attempt to subscribe once again */ - if (_gyro_count == 0) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); - fds[0].fd = _gyro_sub[0]; + if (_gyro.subscription_count == 0) { + init_sensor_class(ORB_ID(sensor_gyro), _gyro); } + usleep(1000); + continue; } @@ -2189,60 +2362,38 @@ Sensors::task_main() /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); - /* the timestamp of the raw struct is updated by the gyro_poll() method */ - /* copy most recent sensor data */ + /* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro + * a mandatory sensor) */ gyro_poll(raw); accel_poll(raw); mag_poll(raw); baro_poll(raw); - // FIXME TODO: this needs more thinking, otherwise we spam the console and keep switching. - /* Work out if main gyro timed out and fail over to alternate gyro. - * However, don't do this if the secondary is not available. */ - if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) { - if (fds[0].fd == _gyro_sub[0]) { - PX4_WARN("gyro0 has timed out"); - } - - /* If the secondary failed as well, go to the tertiary, also only if available. */ - if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) { - fds[0].fd = _gyro_sub[2]; - - if (!_hil_enabled) { - PX4_WARN("failing over to third gyro"); - } - - } else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) { - fds[0].fd = _gyro_sub[1]; - - if (!_hil_enabled) { - PX4_WARN("failing over to second gyro"); - } - } - } /* check battery voltage */ adc_poll(raw); diff_pres_poll(raw); - /* Inform other processes that new data is available to copy */ if (_publishing && raw.timestamp > 0) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + + check_failover(_accel, "Accel"); + check_failover(_gyro, "Gyro"); + check_failover(_mag, "Mag"); + check_failover(_baro, "Baro"); + + //check_vibration(); //disabled for now, as it does not seem to be reliable } /* keep adding sensors as long as we are not armed, * when not adding sensors poll for param updates */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { - _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub); - - _mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub); - - _accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub); - - _baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub); - + init_sensor_class(ORB_ID(sensor_gyro), _gyro); + init_sensor_class(ORB_ID(sensor_mag), _mag); + init_sensor_class(ORB_ID(sensor_accel), _accel); + init_sensor_class(ORB_ID(sensor_baro), _baro); _last_config_update = hrt_absolute_time(); } else { @@ -2260,22 +2411,20 @@ Sensors::task_main() perf_end(_loop_perf); } - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { - if (_gyro_sub[i] >= 0) { - orb_unsubscribe(_gyro_sub[i]); - } + for (unsigned i = 0; i < _gyro.subscription_count; i++) { + orb_unsubscribe(_gyro.subscription[i]); + } - if (_accel_sub[i] >= 0) { - orb_unsubscribe(_accel_sub[i]); - } + for (unsigned i = 0; i < _accel.subscription_count; i++) { + orb_unsubscribe(_accel.subscription[i]); + } - if (_mag_sub[i] >= 0) { - orb_unsubscribe(_mag_sub[i]); - } + for (unsigned i = 0; i < _mag.subscription_count; i++) { + orb_unsubscribe(_mag.subscription[i]); + } - if (_baro_sub[i] >= 0) { - orb_unsubscribe(_baro_sub[i]); - } + for (unsigned i = 0; i < _baro.subscription_count; i++) { + orb_unsubscribe(_baro.subscription[i]); } orb_unsubscribe(_rc_sub); @@ -2316,6 +2465,19 @@ Sensors::start() return OK; } +void Sensors::print_status() +{ + PX4_INFO("gyro status:"); + _gyro.voter.print(); + PX4_INFO("accel status:"); + _accel.voter.print(); + PX4_INFO("mag status:"); + _mag.voter.print(); + PX4_INFO("baro status:"); + _baro.voter.print(); +} + + int sensors_main(int argc, char *argv[]) { if (argc < 2) { @@ -2360,7 +2522,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - PX4_INFO("is running"); + sensors::g_sensors->print_status(); return 0; } else {