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

ekf2: remove use of arm status

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-11 14:53:55 +02:00
committed by Lorenz Meier
parent 4b8c7201b7
commit 4480e4206d
1 changed files with 5 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+5 -14
View File
@@ -141,7 +141,7 @@ private:
int _actuator_armed_sub = -1; int _actuator_armed_sub = -1;
int _vehicle_land_detected_sub = -1; int _vehicle_land_detected_sub = -1;
bool _prev_motors_armed = false; // motors armed status from the previous frame bool _prev_landed = true; // landed status from the previous frame
float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration float _acc_hor_filt = 0.0f; // low-pass filtered horizontal acceleration
@@ -349,7 +349,6 @@ void Ekf2::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); _optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor)); _range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
_actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
px4_pollfd_struct_t fds[2] = {}; px4_pollfd_struct_t fds[2] = {};
@@ -369,7 +368,7 @@ void Ekf2::task_main()
airspeed_s airspeed = {}; airspeed_s airspeed = {};
optical_flow_s optical_flow = {}; optical_flow_s optical_flow = {};
distance_sensor_s range_finder = {}; distance_sensor_s range_finder = {};
actuator_armed_s actuator_armed = {}; vehicle_land_detected_s vehicle_land_detected = {};
while (!_task_should_exit) { while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -402,7 +401,6 @@ void Ekf2::task_main()
bool airspeed_updated = false; bool airspeed_updated = false;
bool optical_flow_updated = false; bool optical_flow_updated = false;
bool range_finder_updated = false; bool range_finder_updated = false;
bool actuator_armed_updated = false;
bool vehicle_land_detected_updated = false; bool vehicle_land_detected_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
@@ -502,17 +500,9 @@ void Ekf2::task_main()
_ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance); _ekf.setRangeData(range_finder.timestamp, &range_finder.current_distance);
} }
orb_check(_actuator_armed_sub, &actuator_armed_updated);
if (actuator_armed_updated) {
orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
_ekf.set_arm_status(actuator_armed.armed);
}
orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated); orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
if (vehicle_land_detected_updated) { if (vehicle_land_detected_updated) {
struct vehicle_land_detected_s vehicle_land_detected = {};
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected); orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
_ekf.set_in_air_status(!vehicle_land_detected.landed); _ekf.set_in_air_status(!vehicle_land_detected.landed);
} }
@@ -780,12 +770,13 @@ void Ekf2::task_main()
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
} }
// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected // save the declination to the EKF2_MAG_DECL parameter when a land event is detected
if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) { if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) {
float decl_deg; float decl_deg;
_ekf.copy_mag_decl_deg(&decl_deg); _ekf.copy_mag_decl_deg(&decl_deg);
_mag_declination_deg.set(decl_deg); _mag_declination_deg.set(decl_deg);
} }
_prev_landed = vehicle_land_detected.landed;
// publish replay message if in replay mode // publish replay message if in replay mode
bool publish_replay_message = (bool)_param_record_replay_msg.get(); bool publish_replay_message = (bool)_param_record_replay_msg.get();