From aa2f3a6624eb55799856babced2871ce0290fda5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 Mar 2020 18:27:02 -0400 Subject: [PATCH] sensors: complete move to uORB::Subscription --- Tools/uorb_graph/create.py | 1 - src/modules/sensors/sensors.cpp | 1 - src/modules/sensors/voted_sensors_update.cpp | 101 ++++--------------- src/modules/sensors/voted_sensors_update.h | 45 +++------ 4 files changed, 34 insertions(+), 114 deletions(-) diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index a231e3f457..a9210f911a 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -220,7 +220,6 @@ class Graph(object): # (the expectation is that the previous matching ORB_ID() will be passed # to this, so that we can ignore it) special_cases_sub = [ - ('sensors', r'voted_sensors_update\.cpp$', r'\binitSensorClass\b\(([^,)]+)', r'^meta$'), ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d52417aa85..96fee8ff1f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -441,7 +441,6 @@ void Sensors::Run() sub.unregisterCallback(); } - _voted_sensors_update.deinit(); exit_and_cleanup(); return; } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 803ce68c4f..62be97492e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -50,6 +50,7 @@ using namespace sensors; using namespace matrix; +using math::radians; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) : ModuleParams(nullptr), _parameters(parameters), _hil_enabled(hil_enabled), _mag_compensator(this) @@ -86,33 +87,15 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) void VotedSensorsUpdate::initializeSensors() { - initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX); - initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX); - initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX); -} - -void VotedSensorsUpdate::deinit() -{ - for (int i = 0; i < _gyro.subscription_count; i++) { - orb_unsubscribe(_gyro.subscription[i]); - } - - for (int i = 0; i < _accel.subscription_count; i++) { - orb_unsubscribe(_accel.subscription[i]); - } - - for (int i = 0; i < _mag.subscription_count; i++) { - orb_unsubscribe(_mag.subscription[i]); - } + initSensorClass(_gyro, GYRO_COUNT_MAX); + initSensorClass(_accel, ACCEL_COUNT_MAX); + initSensorClass(_mag, MAG_COUNT_MAX); } void VotedSensorsUpdate::parametersUpdate() { /* fine tune board offset */ - Dcmf board_rotation_offset = Eulerf( - M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); + const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}}; _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation); @@ -344,7 +327,7 @@ void VotedSensorsUpdate::parametersUpdate() sensor_mag_s report{}; - if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) { + if (!_mag.subscription[topic_instance].copy(&report)) { continue; } @@ -486,27 +469,14 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 }; for (int uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) { - bool accel_updated; - orb_check(_accel.subscription[uorb_index], &accel_updated); - - if (accel_updated) { - sensor_accel_integrated_s accel_report; - int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report); + sensor_accel_integrated_s accel_report; - if (ret != PX4_OK || accel_report.timestamp == 0) { - continue; //ignore invalid data - } - - if (!_accel.enabled[uorb_index]) { - continue; - } + if (_accel.enabled[uorb_index] && _accel.subscription[uorb_index].update(&accel_report)) { // First publication with data if (_accel.priority[uorb_index] == 0) { - int32_t priority = 0; - orb_priority(_accel.subscription[uorb_index], &priority); - _accel.priority[uorb_index] = (uint8_t)priority; + _accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority(); } _accel_device_id[uorb_index] = accel_report.device_id; @@ -567,27 +537,13 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 }; for (int uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) { - bool gyro_updated; - orb_check(_gyro.subscription[uorb_index], &gyro_updated); - - if (gyro_updated) { - sensor_gyro_integrated_s gyro_report; + sensor_gyro_integrated_s gyro_report; - int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report); - - if (ret != PX4_OK || gyro_report.timestamp == 0) { - continue; //ignore invalid data - } - - if (!_gyro.enabled[uorb_index]) { - continue; - } + if (_gyro.enabled[uorb_index] && _gyro.subscription[uorb_index].update(&gyro_report)) { // First publication with data if (_gyro.priority[uorb_index] == 0) { - int32_t priority = 0; - orb_priority(_gyro.subscription[uorb_index], &priority); - _gyro.priority[uorb_index] = (uint8_t)priority; + _gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority(); } _gyro_device_id[uorb_index] = gyro_report.device_id; @@ -646,27 +602,13 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) { for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) { - bool mag_updated; - orb_check(_mag.subscription[uorb_index], &mag_updated); - - if (mag_updated) { - sensor_mag_s mag_report{}; - - int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report); + sensor_mag_s mag_report; - if (ret != PX4_OK || mag_report.timestamp == 0) { - continue; //ignore invalid data - } - - if (!_mag.enabled[uorb_index]) { - continue; - } + if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) { // First publication with data if (_mag.priority[uorb_index] == 0) { - int32_t priority = 0; - orb_priority(_mag.subscription[uorb_index], &priority); - _mag.priority[uorb_index] = (uint8_t)priority; + _mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority(); /* force a scale and offset update the first time we get data */ parametersUpdate(); @@ -678,7 +620,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) */ continue; } - } Vector3f vect(mag_report.x, mag_report.y, mag_report.z); @@ -781,25 +722,21 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na return false; } -void VotedSensorsUpdate::initSensorClass(const struct orb_metadata *meta, SensorData &sensor_data, - uint8_t sensor_count_max) +void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max) { int max_sensor_index = -1; for (unsigned i = 0; i < sensor_count_max; i++) { - if (orb_exists(meta, i) != 0) { - continue; - } max_sensor_index = i; - if (sensor_data.subscription[i] < 0) { - sensor_data.subscription[i] = orb_subscribe_multi(meta, i); + if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) { + sensor_data.advertised[i] = true; if (i > 0) { /* the first always exists, but for each further sensor, add a new validator */ if (!sensor_data.voter.add_new_validator()) { - PX4_ERR("failed to add validator for sensor %s %i", meta->o_name, i); + PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i); } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 3b4e1177b9..ab89386b9e 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -95,11 +95,6 @@ public: */ void initializeSensors(); - /** - * deinitialize the object (we cannot use the destructor because it is called on the wrong thread) - */ - void deinit(); - void printStatus(); /** @@ -159,31 +154,21 @@ public: private: struct SensorData { - SensorData() - : last_best_vote(0), - subscription_count(0), - voter(1), - last_failover_count(0) - { - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { - enabled[i] = true; - subscription[i] = -1; - priority[i] = 0; - } - } - - bool enabled[SENSOR_COUNT_MAX]; - - int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ - uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */ - uint8_t last_best_vote; /**< index of the latest best vote */ - int subscription_count; - DataValidatorGroup voter; - unsigned int last_failover_count; + SensorData() = delete; + explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {} + + uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */ + DataValidatorGroup voter{1}; + unsigned int last_failover_count{0}; + uint8_t priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */ + uint8_t last_best_vote{0}; /**< index of the latest best vote */ + uint8_t subscription_count{0}; + bool enabled[SENSOR_COUNT_MAX] {true, true, true, true}; + bool advertised[SENSOR_COUNT_MAX] {false, false, false, false}; matrix::Vector3f power_compensation[SENSOR_COUNT_MAX]; }; - void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); + void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); /** * Poll the accelerometer for updated data. @@ -215,9 +200,9 @@ private: */ bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type); - SensorData _accel {}; - SensorData _gyro {}; - SensorData _mag {}; + SensorData _accel{ORB_ID::sensor_accel_integrated}; + SensorData _gyro{ORB_ID::sensor_gyro_integrated}; + SensorData _mag{ORB_ID::sensor_mag}; orb_advert_t _mavlink_log_pub{nullptr};