fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors app: Initialize class count, remove magic numbers, ensure that the sensor combined struct cannot overflow

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-30 11:52:31 +02:00
parent 0732ec650f
commit a7e3232e7f
1 changed files with 40 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+40 -26
View File
@@ -129,6 +129,8 @@
#define STICK_ON_OFF_LIMIT 0.75f #define STICK_ON_OFF_LIMIT 0.75f
#define MAG_ROT_VAL_INTERNAL -1 #define MAG_ROT_VAL_INTERNAL -1
#define SENSOR_COUNT_MAX 3
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
# undef ERROR # undef ERROR
@@ -201,21 +203,21 @@ private:
bool _hil_enabled; /**< if true, HIL is active */ bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */ bool _publishing; /**< if true, we are publishing sensor data */
int _gyro_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw gyro0 data subscription */ int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */
int _accel_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw accel0 data subscription */ int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */
int _mag_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw mag0 data subscription */ int _mag_sub[SENSOR_COUNT_MAX]; /**< raw mag data subscription */
int _baro_sub[ORB_MULTI_MAX_INSTANCES - 1]; /**< raw baro0 data subscription */ int _baro_sub[SENSOR_COUNT_MAX]; /**< raw baro data subscription */
unsigned _gyro_count; /**< raw gyro0 data subscription */ unsigned _gyro_count; /**< raw gyro data count */
unsigned _accel_count; /**< raw accel0 data subscription */ unsigned _accel_count; /**< raw accel data count */
unsigned _mag_count; /**< raw mag0 data subscription */ unsigned _mag_count; /**< raw mag data count */
unsigned _baro_count; /**< raw baro0 data subscription */ unsigned _baro_count; /**< raw baro data count */
int _rc_sub; /**< raw rc channels data subscription */ int _rc_sub; /**< raw rc channels data subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */
int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< notification of parameter updates */ int _params_sub; /**< notification of parameter updates */
int _rc_parameter_map_sub; /**< rc parameter map subscription */ int _rc_parameter_map_sub; /**< rc parameter map subscription */
int _manual_control_sub; /**< notification of manual control updates */ int _manual_control_sub; /**< notification of manual control updates */
orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */
@@ -236,7 +238,7 @@ private:
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
@@ -362,8 +364,8 @@ private:
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
void init_sensor_class(const struct orb_metadata *meta, int *subs, int init_sensor_class(const struct orb_metadata *meta, int *subs,
unsigned *priorities, unsigned *errcount); unsigned *priorities, unsigned *errcount);
/** /**
* Update our local parameter cache. * Update our local parameter cache.
@@ -1226,7 +1228,7 @@ Sensors::parameter_update_poll(bool forced)
unsigned accel_count = 0; unsigned accel_count = 0;
/* run through all gyro sensors */ /* run through all gyro sensors */
for (unsigned s = 0; s < 3; s++) { for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
res = ERROR; res = ERROR;
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
@@ -1240,7 +1242,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false; bool config_ok = false;
/* run through all stored calibrations */ /* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */ /* initially status is ok per config */
failed = false; failed = false;
@@ -1292,7 +1294,7 @@ Sensors::parameter_update_poll(bool forced)
} }
/* run through all accel sensors */ /* run through all accel sensors */
for (unsigned s = 0; s < 3; s++) { for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
res = ERROR; res = ERROR;
(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@@ -1306,7 +1308,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false; bool config_ok = false;
/* run through all stored calibrations */ /* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */ /* initially status is ok per config */
failed = false; failed = false;
@@ -1358,7 +1360,7 @@ Sensors::parameter_update_poll(bool forced)
} }
/* run through all mag sensors */ /* run through all mag sensors */
for (unsigned s = 0; s < 3; s++) { for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
/* set a valid default rotation (same as board). /* set a valid default rotation (same as board).
* if the mag is configured, this might be replaced * if the mag is configured, this might be replaced
@@ -1379,7 +1381,7 @@ Sensors::parameter_update_poll(bool forced)
bool config_ok = false; bool config_ok = false;
/* run through all stored calibrations */ /* run through all stored calibrations */
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
/* initially status is ok per config */ /* initially status is ok per config */
failed = false; failed = false;
@@ -1934,15 +1936,23 @@ Sensors::task_main_trampoline(int argc, char *argv[])
sensors::g_sensors->task_main(); sensors::g_sensors->task_main();
} }
void int
Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
unsigned *priorities, unsigned *errcount) unsigned *priorities, unsigned *errcount)
{ {
for (unsigned i = 0; i < (unsigned)orb_group_count(meta); i++) { unsigned group_count = orb_group_count(meta);
if (group_count > SENSOR_COUNT_MAX) {
group_count = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < group_count; i++) {
subs[i] = orb_subscribe_multi(meta, i); subs[i] = orb_subscribe_multi(meta, i);
orb_priority(subs[i], (int32_t*)&priorities[i]); orb_priority(subs[i], (int32_t*)&priorities[i]);
errcount[i] = 100000; errcount[i] = 100000;
} }
return group_count;
} }
void void
@@ -1977,20 +1987,24 @@ Sensors::task_main()
struct sensor_combined_s raw = {}; struct sensor_combined_s raw = {};
/* ensure no overflows can occur */
static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
/* /*
* do subscriptions * do subscriptions
*/ */
init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]); &raw.gyro_priority[0], &raw.gyro_errcount[0]);
init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]); &raw.baro_priority[0], &raw.baro_errcount[0]);
_rc_sub = orb_subscribe(ORB_ID(input_rc)); _rc_sub = orb_subscribe(ORB_ID(input_rc));