Browse Source

sensors true airspeed fix

- _voted_sensors_update.baro_pressure() is already in pascals
sbg
Daniel Agar 8 years ago committed by Lorenz Meier
parent
commit
80c8130f07
  1. 10
      src/modules/sensors/sensors.cpp
  2. 2
      src/modules/sensors/voted_sensors_update.h

10
src/modules/sensors/sensors.cpp

@ -383,6 +383,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) @@ -383,6 +383,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
/* push data into validator */
_airspeed_validator.put(_airspeed.timestamp, _diff_pres.differential_pressure_raw_pa, _diff_pres.error_count,
ORB_PRIO_HIGH);
_airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
/* don't risk to feed negative airspeed into the system */
@ -390,11 +391,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) @@ -390,11 +391,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
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));
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _voted_sensors_update.baro_pressure(),
_voted_sensors_update.baro_pressure(), 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));
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _voted_sensors_update.baro_pressure(),
_voted_sensors_update.baro_pressure(), air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
_airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;

2
src/modules/sensors/voted_sensors_update.h

@ -246,7 +246,7 @@ private: @@ -246,7 +246,7 @@ private:
orb_advert_t _mavlink_log_pub = nullptr;
float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
float _last_best_baro_pressure = 0.f; /**< pressure from last best baro */
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 */

Loading…
Cancel
Save