diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6e6f0abe78..cc6ccb0755 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -629,11 +629,13 @@ void EKF2::Run() new_ev_data_received = true; } - bool vehicle_land_detected_updated = _vehicle_land_detected_sub.updated(); + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; - if (vehicle_land_detected_updated) { - if (_vehicle_land_detected_sub.copy(&_vehicle_land_detected)) { - _ekf.set_in_air_status(!_vehicle_land_detected.landed); + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _ekf.set_in_air_status(!vehicle_land_detected.landed); + _landed = vehicle_land_detected.landed; + _in_ground_effect = vehicle_land_detected.in_ground_effect; } } @@ -757,9 +759,9 @@ void EKF2::Run() // _had_valid_terrain is used to make sure that we don't fall back to using this option // if we temporarily lose terrain data due to the distance sensor getting out of range - } else if (vehicle_land_detected_updated && !_had_valid_terrain) { + } else if (!_had_valid_terrain) { // update ground effect flag based on land detector state - _ekf.set_gnd_effect_flag(_vehicle_land_detected.in_ground_effect); + _ekf.set_gnd_effect_flag(_in_ground_effect); } } else { @@ -932,7 +934,7 @@ void EKF2::Run() /* Check and save learned magnetometer bias estimates */ // Check if conditions are OK for learning of magnetometer bias values - if (!_vehicle_land_detected.landed && // not on ground + if (!_landed && // not on ground (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && // vehicle is armed !status.filter_fault_flags && // there are no filter faults control_status.flags.mag_3D) { // the EKF is operating in the correct mode diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 74beb8d8a9..075581df3d 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -213,9 +213,11 @@ private: uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; int _range_finder_sub_index = -1; // index for downward-facing range finder subscription - vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; + bool _landed{true}; + bool _in_ground_effect{false}; + uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};