fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 03:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors : decouple maximum sensor count and allow flexible maximums

Browse Source
This commit is contained in:
Mohammed Kabir
2017-04-22 15:26:20 +02:00
committed by Kabir Mohammed
parent f0accd39f0
commit 57aa41df2c
5 changed files with 70 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/common.h
+9 -1
View File
@@ -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 */
src/modules/sensors/temperature_compensation.cpp
+14 -14
View File
@@ -54,7 +54,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
/* 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 &para
/* 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 &para
/* 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<typename T>
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) {
src/modules/sensors/temperature_compensation.h
+13 -9
View File
@@ -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<typename T>
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);
};
}
src/modules/sensors/voted_sensors_update.cpp
+28 -19
View File
@@ -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++) {
src/modules/sensors/voted_sensors_update.h
+6 -7
View File
@@ -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? */