fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move pressure altitude from baros to sensors module

Browse Source
This commit is contained in:
Daniel Agar
2018-04-11 23:26:58 -04:00
parent bdf4f19867
commit 5dc23def2a
22 changed files with 55 additions and 794 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+2 -14
View File
@@ -57,7 +57,6 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
@@ -521,7 +520,6 @@ void Ekf2::run()
int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
int status_sub = orb_subscribe(ORB_ID(vehicle_status));
int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
int landing_target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose));
bool imu_bias_reset_request = false;
@@ -554,8 +552,6 @@ void Ekf2::run()
vehicle_attitude_s ev_att = {};
vehicle_status_s vehicle_status = {};
sensor_selection_s sensor_selection = {};
sensor_baro_s sensor_baro = {};
sensor_baro.pressure = 1013.5f; // initialise pressure to sea level
landing_target_pose_s landing_target_pose = {};
while (!should_exit()) {
@@ -588,7 +584,6 @@ void Ekf2::run()
bool gps_updated = false;
bool airspeed_updated = false;
bool baro_updated = false;
bool sensor_selection_updated = false;
bool optical_flow_updated = false;
bool range_finder_updated = false;
@@ -620,12 +615,6 @@ void Ekf2::run()
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
}
orb_check(sensor_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
}
orb_check(sensor_selection_sub, &sensor_selection_updated);
// Always update sensor selction first time through if time stamp is non zero
@@ -808,8 +797,8 @@ void Ekf2::run()
float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
// estimate air density assuming typical 20degC ambient temperature
const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
const float rho = pressure_to_density * sensor_baro.pressure;
const float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
const float rho = pressure_to_density * sensors.baro_pressure_pa;
_ekf.set_air_density(rho);
// calculate static pressure error = Pmeas - Ptruth
@@ -1454,7 +1443,6 @@ void Ekf2::run()
orb_unsubscribe(vehicle_land_detected_sub);
orb_unsubscribe(status_sub);
orb_unsubscribe(sensor_selection_sub);
orb_unsubscribe(sensor_baro_sub);
orb_unsubscribe(landing_target_pose_sub);
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {