From 3bcb710da95258de311d754de78b30ea9fc81d38 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Mar 2017 09:17:06 +1100 Subject: [PATCH] voted_sensors_update: publish sensor selections (+3 squashed commits) Squashed commits: [290660d] voted_sensors_update: revert the new_accel_data check (and others) The check removed the ability to detect sensor timeouts. [c8dc7ad] sensors: publish changes to sensor selection [dd90dec] sensors: ensure all sensor selections published first time --- src/modules/sensors/voted_sensors_update.cpp | 44 +++++++++++++++++++- src/modules/sensors/voted_sensors_update.h | 10 +++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index aa9de07ba6..7bf9afc289 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -99,6 +99,7 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) initialize_sensors(); _corrections_changed = true; //make sure to initially publish the corrections topic + _selection_changed = true; return 0; } @@ -556,6 +557,8 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) _accel.priority[uorb_index] = (uint8_t)priority; } + _accel_device_id[uorb_index] = accel_report.device_id; + math::Vector<3> accel_data; if (accel_report.integral_dt != 0) { @@ -625,6 +628,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) _corrections_changed = true; } + if (_selection.accel_device_id != _accel_device_id[best_index]) { + _selection_changed = true; + _selection.accel_device_id = _accel_device_id[best_index]; + } + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index]; } @@ -656,6 +664,8 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) _gyro.priority[uorb_index] = (uint8_t)priority; } + _gyro_device_id[uorb_index] = gyro_report.device_id; + math::Vector<3> gyro_rate; if (gyro_report.integral_dt != 0) { @@ -726,6 +736,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) _corrections_changed = true; } + if (_selection.gyro_device_id != _gyro_device_id[best_index]) { + _selection_changed = true; + _selection.gyro_device_id = _gyro_device_id[best_index]; + } + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index]; } @@ -754,6 +769,8 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) _mag.priority[i] = (uint8_t)priority; } + _mag_device_id[i] = mag_report.device_id; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); vect = _mag_rotation[i] * vect; @@ -776,6 +793,11 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2]; _mag.last_best_vote = (uint8_t)best_index; } + + if (_selection.mag_device_id != _mag_device_id[best_index]) { + _selection_changed = true; + _selection.mag_device_id = _mag_device_id[best_index]; + } } void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) @@ -815,6 +837,8 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) _baro.priority[uorb_index] = (uint8_t)priority; } + _baro_device_id[uorb_index] = baro_report.device_id; + got_update = true; math::Vector<3> vect(baro_report.altitude, 0.f, 0.f); @@ -842,6 +866,11 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) _corrections_changed = true; } + if (_selection.baro_device_id != _baro_device_id[best_index]) { + _selection_changed = true; + _selection.baro_device_id = _baro_device_id[best_index]; + } + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ /* @@ -1041,11 +1070,24 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw) } else { orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections); - } _corrections_changed = false; } + + // publish sensor selection if changed + if (_selection_changed) { + _selection.timestamp = hrt_absolute_time(); + + if (_sensor_selection_pub == nullptr) { + _sensor_selection_pub = orb_advertise(ORB_ID(sensor_selection), &_selection); + + } else { + orb_publish(ORB_ID(sensor_selection), _sensor_selection_pub, &_selection); + } + + _selection_changed = false; + } } void VotedSensorsUpdate::check_failover() diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 5caf8b5d1c..642ee3c11b 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -269,6 +270,15 @@ private: orb_advert_t _sensor_correction_pub = nullptr; /**< handle to the sensor correction uORB topic */ bool _corrections_changed = false; + /* sensor selection publication */ + struct sensor_selection_s _selection = {}; /**< struct containing the sensor selection to be published to the uORB*/ + orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */ + bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */ + uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; + uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {}; + uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {}; + uint32_t _mag_device_id[SENSOR_COUNT_MAX] = {}; + static const double _msl_pressure; /** average sea-level pressure in kPa */ };