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

sensors app: Keep looking for new sensors until system is fully booted

Browse Source
This commit is contained in:
Lorenz Meier 2015-09-07 10:16:50 +02:00
parent 52a2946827
commit ad2058427d
1 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/modules/sensors/sensors.cpp
View File

@ -202,6 +202,7 @@ private:
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
bool _armed; /**< arming status of the vehicle */
int _gyro_sub[SENSOR_COUNT_MAX]; /**< raw gyro data subscription */
int _accel_sub[SENSOR_COUNT_MAX]; /**< raw accel data subscription */
@ -485,6 +486,7 @@ Sensors::Sensors() :
_sensors_task(-1),
_hil_enabled(false),
_publishing(true),
_armed(false),
/* subscriptions */
_gyro_sub{-1, -1, -1},
@ -520,6 +522,14 @@ Sensors::Sensors() :
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* initialize subscriptions */
for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
_gyro_sub[i] = -1;
_accel_sub[i] = -1;
_mag_sub[i] = -1;
_baro_sub[i] = -1;
}
memset(&_rc, 0, sizeof(_rc));
memset(&_diff_pres, 0, sizeof(_diff_pres));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
@ -1189,16 +1199,17 @@ Sensors::vehicle_control_mode_poll()
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
_hil_enabled = true;
_publishing = false;
_armed = vcontrol_mode.flag_armed;
/* switching from HIL to non-HIL mode */
} else if (!_publishing && !_hil_enabled) {
_hil_enabled = false;
_publishing = true;
_armed = vcontrol_mode.flag_armed;
}
}
}
@ -1947,9 +1958,10 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
}
for (unsigned i = 0; i < group_count; i++) {
subs[i] = orb_subscribe_multi(meta, i);
orb_priority(subs[i], (int32_t*)&priorities[i]);
errcount[i] = 100000;
if (subs[i] < 0) {
subs[i] = orb_subscribe_multi(meta, i);
orb_priority(subs[i], (int32_t*)&priorities[i]);
}
}
return group_count;
@ -2074,6 +2086,21 @@ Sensors::task_main()
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* keep adding sensors as long as we are not armed */
if (!_armed) {
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
}
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
gyro_poll(raw);