void
Sensors::task_main()
{
int ret = 0;
if (!_hil_enabled) {
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_BEBOP)
adc_init();
#endif
}
struct sensor_combined_s raw = {};
struct sensor_preflight_s preflt = {};
_rc_update.init();
_voted_sensors_update.init(raw);
/* (re)load params and calibration */
parameter_update_poll(true);
/*
* do subscriptions
*/
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_battery.reset(&_battery_status);
/* get a set of initial values */
_voted_sensors_update.sensors_poll(raw);
diff_pres_poll(raw);
_rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */);
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* advertise the sensor_preflight topic and make the initial publication */
preflt.accel_inconsistency_m_s_s = 0.0f;
preflt.gyro_inconsistency_rad_s = 0.0f;
_sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt);
/* wakeup source */
px4_pollfd_struct_t poll_fds = {};
poll_fds.events = POLLIN;
_task_should_exit = false;
uint64_t last_config_update = hrt_absolute_time();
while (!_task_should_exit) {
/* use the best-voted gyro to pace output */
poll_fds.fd = _voted_sensors_update.best_gyro_fd();
/* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms,
* if a gyro fails) */
int pret = px4_poll(&poll_fds, 1, 50);
/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
/* if the polling operation failed because no gyro sensor is available yet,
* then attempt to subscribe once again
*/
if (_voted_sensors_update.num_gyros() == 0) {
_voted_sensors_update.initialize_sensors();
}
usleep(1000);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
* a mandatory sensor) */
_voted_sensors_update.sensors_poll(raw);
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
if (raw.timestamp > 0) {
_voted_sensors_update.set_relative_timestamps(raw);
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
_voted_sensors_update.check_failover();
/* If the the vehicle is disarmed calculate the length of the maximum difference between
* IMU units as a consistency metric and publish to the sensor preflight topic
*/
if (!_armed) {
_voted_sensors_update.calc_accel_inconsistency(preflt);
_voted_sensors_update.calc_gyro_inconsistency(preflt);
orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt);
}
//_voted_sensors_update.check_vibration(); //disabled for now, as it does not seem to be reliable
}
/* keep adding sensors as long as we are not armed,
* when not adding sensors poll for param updates
*/
if (!_armed && hrt_elapsed_time(&last_config_update) > 500 * 1000) {
_voted_sensors_update.initialize_sensors();
last_config_update = hrt_absolute_time();
} else {
/* check parameters for updates */
parameter_update_poll();
/* check rc parameter map for updates */
_rc_update.rc_parameter_map_poll(_parameter_handles);
}
/* Look for new r/c input data */
_rc_update.rc_poll(_parameter_handles);
perf_end(_loop_perf);
}
orb_unsubscribe(_diff_pres_sub);
orb_unsubscribe(_vcontrol_mode_sub);
orb_unsubscribe(_params_sub);
orb_unsubscribe(_actuator_ctrl_0_sub);
orb_unadvertise(_sensor_pub);
_rc_update.deinit();
_voted_sensors_update.deinit();
_sensors_task = -1;
px4_task_exit(ret);
}
