From 57aa41df2c59a3d3636e63273dfc8e08890919ba Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sat, 22 Apr 2017 15:26:20 +0200 Subject: [PATCH] sensors : decouple maximum sensor count and allow flexible maximums --- src/modules/sensors/common.h | 10 +++- .../sensors/temperature_compensation.cpp | 28 +++++------ .../sensors/temperature_compensation.h | 22 +++++---- src/modules/sensors/voted_sensors_update.cpp | 47 +++++++++++-------- src/modules/sensors/voted_sensors_update.h | 13 +++-- 5 files changed, 70 insertions(+), 50 deletions(-) diff --git a/src/modules/sensors/common.h b/src/modules/sensors/common.h index 6d6f6b6c56..93e8589716 100644 --- a/src/modules/sensors/common.h +++ b/src/modules/sensors/common.h @@ -43,6 +43,14 @@ namespace sensors { -static const int SENSOR_COUNT_MAX = 3; +constexpr uint8_t MAG_COUNT_MAX = 4; +constexpr uint8_t GYRO_COUNT_MAX = 3; +constexpr uint8_t ACCEL_COUNT_MAX = 3; +constexpr uint8_t BARO_COUNT_MAX = 3; + +constexpr uint8_t SENSOR_COUNT_MAX = math::max(MAG_COUNT_MAX, + math::max(GYRO_COUNT_MAX, + math::max(ACCEL_COUNT_MAX, + BARO_COUNT_MAX))); } /* namespace sensors */ diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index ed4185c69a..4b7d15d4e1 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -54,7 +54,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ /* rate gyro calibration parameters */ parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE"); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { sprintf(nbuf, "TC_G%d_ID", j); parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf); @@ -82,7 +82,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ /* accelerometer calibration parameters */ parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE"); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { sprintf(nbuf, "TC_A%d_ID", j); parameter_handles.accel_cal_handles[j].ID = param_find(nbuf); @@ -110,7 +110,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ /* barometer calibration parameters */ parameter_handles.baro_tc_enable = param_find("TC_B_ENABLE"); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { sprintf(nbuf, "TC_B%d_ID", j); parameter_handles.baro_cal_handles[j].ID = param_find(nbuf); sprintf(nbuf, "TC_B%d_X5", j); @@ -152,7 +152,7 @@ int TemperatureCompensation::parameters_update() /* rate gyro calibration parameters */ param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable)); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(_parameters.gyro_cal_data[j].ID)) == PX4_OK) { param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(_parameters.gyro_cal_data[j].ref_temp)); param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(_parameters.gyro_cal_data[j].min_temp)); @@ -183,7 +183,7 @@ int TemperatureCompensation::parameters_update() /* accelerometer calibration parameters */ param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable)); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) { param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp)); param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp)); @@ -214,7 +214,7 @@ int TemperatureCompensation::parameters_update() /* barometer calibration parameters */ param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable)); - for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { if (param_get(parameter_handles.baro_cal_handles[j].ID, &(_parameters.baro_cal_data[j].ID)) == PX4_OK) { param_get(parameter_handles.baro_cal_handles[j].ref_temp, &(_parameters.baro_cal_data[j].ref_temp)); param_get(parameter_handles.baro_cal_handles[j].min_temp, &(_parameters.baro_cal_data[j].min_temp)); @@ -323,7 +323,7 @@ int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_in return 0; } - return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data); + return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX); } int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance) @@ -332,7 +332,7 @@ int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_i return 0; } - return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data); + return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX); } int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance) @@ -341,14 +341,14 @@ int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_in return 0; } - return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data); + return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data, BARO_COUNT_MAX); } template int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, - const T *sensor_cal_data) + const T *sensor_cal_data, uint8_t sensor_count_max) { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + for (int i = 0; i < sensor_count_max; ++i) { if (device_id == sensor_cal_data[i].ID) { sensor_data.device_mapping[topic_instance] = i; return i; @@ -449,7 +449,7 @@ void TemperatureCompensation::print_status() PX4_INFO(" gyro: enabled: %i", _parameters.gyro_tc_enable); if (_parameters.gyro_tc_enable == 1) { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + for (int i = 0; i < GYRO_COUNT_MAX; ++i) { uint8_t mapping = _gyro_data.device_mapping[i]; if (_gyro_data.device_mapping[i] != 255) { @@ -461,7 +461,7 @@ void TemperatureCompensation::print_status() PX4_INFO(" accel: enabled: %i", _parameters.accel_tc_enable); if (_parameters.accel_tc_enable == 1) { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + for (int i = 0; i < ACCEL_COUNT_MAX; ++i) { uint8_t mapping = _accel_data.device_mapping[i]; if (_accel_data.device_mapping[i] != 255) { @@ -473,7 +473,7 @@ void TemperatureCompensation::print_status() PX4_INFO(" baro: enabled: %i", _parameters.baro_tc_enable); if (_parameters.baro_tc_enable == 1) { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + for (int i = 0; i < BARO_COUNT_MAX; ++i) { uint8_t mapping = _baro_data.device_mapping[i]; if (_baro_data.device_mapping[i] != 255) { diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index f924d27950..fb68a8e94d 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -49,8 +49,12 @@ namespace sensors { -static_assert(SENSOR_COUNT_MAX == 3, - "SENSOR_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); +static_assert(GYRO_COUNT_MAX == 3, + "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); +static_assert(ACCEL_COUNT_MAX == 3, + "ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); +static_assert(BARO_COUNT_MAX == 3, + "BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); /** ** class TemperatureCompensation @@ -182,21 +186,21 @@ private: // create a struct containing all thermal calibration parameters struct Parameters { int gyro_tc_enable; - SensorCalData3D gyro_cal_data[SENSOR_COUNT_MAX]; + SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX]; int accel_tc_enable; - SensorCalData3D accel_cal_data[SENSOR_COUNT_MAX]; + SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX]; int baro_tc_enable; - SensorCalData1D baro_cal_data[SENSOR_COUNT_MAX]; + SensorCalData1D baro_cal_data[BARO_COUNT_MAX]; }; // create a struct containing the handles required to access all calibration parameters struct ParameterHandles { param_t gyro_tc_enable; - SensorCalHandles3D gyro_cal_handles[SENSOR_COUNT_MAX]; + SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX]; param_t accel_tc_enable; - SensorCalHandles3D accel_cal_handles[SENSOR_COUNT_MAX]; + SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX]; param_t baro_tc_enable; - SensorCalHandles1D baro_cal_handles[SENSOR_COUNT_MAX]; + SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX]; }; @@ -268,7 +272,7 @@ private: template static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, - const T *sensor_cal_data); + const T *sensor_cal_data, uint8_t sensor_count_max); }; } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d10cd51cae..875e00d04a 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -105,10 +105,10 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) void VotedSensorsUpdate::initialize_sensors() { - 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); + init_sensor_class(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX); + init_sensor_class(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX); + init_sensor_class(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX); + init_sensor_class(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX); } void VotedSensorsUpdate::deinit() @@ -148,9 +148,9 @@ void VotedSensorsUpdate::parameters_update() /* temperature compensation */ _temperature_compensation.parameters_update(); - for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) { + /* gyro */ + for (unsigned topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) { - /* gyro */ if (topic_instance < _gyro.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data struct gyro_report report; @@ -169,8 +169,12 @@ void VotedSensorsUpdate::parameters_update() } } } + } + + + /* accel */ + for (unsigned topic_instance = 0; topic_instance < ACCEL_COUNT_MAX; ++topic_instance) { - /* accel */ if (topic_instance < _accel.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data struct accel_report report; @@ -189,8 +193,11 @@ void VotedSensorsUpdate::parameters_update() } } } + } + + /* baro */ + for (unsigned topic_instance = 0; topic_instance < BARO_COUNT_MAX; ++topic_instance) { - /* baro */ if (topic_instance < _baro.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data struct baro_report report; @@ -222,7 +229,7 @@ void VotedSensorsUpdate::parameters_update() unsigned accel_cal_found_count = 0; /* run through all gyro sensors */ - for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { + for (unsigned s = 0; s < GYRO_COUNT_MAX; s++) { (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); @@ -237,7 +244,7 @@ void VotedSensorsUpdate::parameters_update() bool config_ok = false; /* run through all stored calibrations that are applied at the driver level*/ - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -296,7 +303,7 @@ void VotedSensorsUpdate::parameters_update() if (gyro_count < gyro_cal_found_count) { // run through all stored calibrations and reset them - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) { int device_id = 0; (void)sprintf(str, "CAL_GYRO%u_ID", i); @@ -305,7 +312,7 @@ void VotedSensorsUpdate::parameters_update() } /* run through all accel sensors */ - for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { + for (unsigned s = 0; s < ACCEL_COUNT_MAX; s++) { (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); @@ -320,7 +327,7 @@ void VotedSensorsUpdate::parameters_update() bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -379,7 +386,7 @@ void VotedSensorsUpdate::parameters_update() if (accel_count < accel_cal_found_count) { // run through all stored calibrations and reset them - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) { int device_id = 0; (void)sprintf(str, "CAL_ACC%u_ID", i); @@ -388,7 +395,7 @@ void VotedSensorsUpdate::parameters_update() } /* run through all mag sensors */ - for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { + for (unsigned s = 0; s < MAG_COUNT_MAX; s++) { /* set a valid default rotation (same as board). * if the mag is configured, this might be replaced @@ -410,7 +417,7 @@ void VotedSensorsUpdate::parameters_update() bool config_ok = false; /* run through all stored calibrations */ - for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { + for (unsigned i = 0; i < MAG_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -936,12 +943,14 @@ bool VotedSensorsUpdate::check_vibration() return ret; } -void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data) +void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, + uint8_t sensor_count_max) { unsigned group_count = orb_group_count(meta); - if (group_count > SENSOR_COUNT_MAX) { - group_count = SENSOR_COUNT_MAX; + if (group_count > sensor_count_max) { + PX4_WARN("Detected %u %s sensors, but will only use %u", group_count, meta->o_name, sensor_count_max); + group_count = sensor_count_max; } for (unsigned i = 0; i < group_count; i++) { diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 1a3221242f..5caf8b5d1c 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -166,7 +166,7 @@ private: unsigned int last_failover_count; }; - void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data); + void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); /** * Poll the accelerometer for updated data. @@ -237,7 +237,6 @@ private: */ bool apply_mag_calibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); - SensorData _gyro; SensorData _accel; SensorData _mag; @@ -245,18 +244,18 @@ private: orb_advert_t _mavlink_log_pub = nullptr; - float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */ + float _last_baro_pressure[BARO_COUNT_MAX]; /**< pressure from last baro sensors */ float _last_best_baro_pressure = 0.0f; /**< pressure from last best baro */ sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ - uint64_t _last_accel_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ - uint64_t _last_mag_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ - uint64_t _last_baro_timestamp[SENSOR_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */ hrt_abstime _vibration_warning_timestamp = 0; bool _vibration_warning = false; math::Matrix<3, 3> _board_rotation = {}; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3, 3> _mag_rotation[SENSOR_COUNT_MAX] = {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ + math::Matrix<3, 3> _mag_rotation[MAG_COUNT_MAX] = {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ const Parameters &_parameters; const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */