forked from Archive/PX4-Autopilot
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
This commit is contained in:
parent
3d3e04cb48
commit
3bcb710da9
|
@ -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()
|
||||
|
|
|
@ -55,6 +55,7 @@
|
|||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_preflight.h>
|
||||
#include <uORB/topics/sensor_correction.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
|
||||
#include <DevMgr.hpp>
|
||||
|
||||
|
@ -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 */
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue