void Sensors::print_status()
{
_voted_sensors_update.print_status();
PX4_INFO("Airspeed status:");
_airspeed_validator.print();
}
void
Sensors::diff_pres_poll(struct sensor_combined_s &raw)
{
bool updated;
orb_check(_diff_pres_sub, &updated);
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_airspeed.timestamp = _diff_pres.timestamp;
/* push data into validator */
_airspeed_validator.put(_airspeed.timestamp, _diff_pres.differential_pressure_raw_pa, _diff_pres.error_count, 100);
#ifdef __PX4_POSIX
_airspeed.confidence = 1.0f;
#else
_airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
#endif
/* don't risk to feed negative airspeed into the system */
_airspeed.indicated_airspeed_m_s = math::max(0.0f,
calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
_airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _voted_sensors_update.baro_pressure() * 1e2f,
_voted_sensors_update.baro_pressure() * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _voted_sensors_update.baro_pressure() * 1e2f,
_voted_sensors_update.baro_pressure() * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
_airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
int instance;
orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &_airspeed, &instance, ORB_PRIO_DEFAULT);
}
}
int
Sensors::parameters_update()
{
/* read the parameter values into _parameters */
int ret = update_parameters(_parameter_handles, _parameters);
if (ret) {
return ret;
}
_rc_update.update_rc_functions();
_voted_sensors_update.parameters_update();
/* update barometer qnh setting */
DevHandle h_baro;
DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) && !defined(__PX4_POSIX_OCPOC)
// TODO: this needs fixing for QURT and Raspberry Pi
if (!h_baro.isValid()) {
if (!_hil_enabled) { // in HIL we don't have a baro
PX4_ERR("no barometer found on %s (%d)", BARO0_DEVICE_PATH, h_baro.getError());
ret = PX4_ERROR;
}
} else {
int baroret = h_baro.ioctl(BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
if (baroret) {
PX4_ERR("qnh for baro could not be set");
ret = PX4_ERROR;
}
}
#endif
return ret;
}
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);
}
void Sensors::print_status()
{
_voted_sensors_update.print_status();
}
