dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf control: move time_last_in_air/on_ground out of GPS control logic

This commit is contained in:
bresch 2021-02-16 15:21:36 +01:00 committed by Mathieu Bresciani
parent b00d4a9237
commit 04844a04ed
3 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/control.cpp
View File

@ -120,8 +120,6 @@ void Ekf::controlFusionModes()
_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us; _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
} }
{ {
// Get range data from buffer and check validity // Get range data from buffer and check validity
const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
@ -599,13 +597,6 @@ void Ekf::controlGpsFusion()
could have been caused by bad GPS. could have been caused by bad GPS.
*/ */
if (!_control_status.flags.in_air) {
_time_last_on_ground_us = _time_last_imu;
} else {
_time_last_in_air = _time_last_imu;
}
const bool recent_takeoff_nav_failure = _control_status.flags.in_air && const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&
!isTimedOut(_time_last_on_ground_us, 30000000) && !isTimedOut(_time_last_on_ground_us, 30000000) &&
isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&

2
EKF/ekf.h
View File

@ -959,8 +959,6 @@ private:
EKFGSF_yaw _yawEstimator; EKFGSF_yaw _yawEstimator;
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec)
uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec)
bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested
uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate

13
EKF/estimator_interface.h
View File

@ -94,7 +94,16 @@ public:
parameters *getParamHandle() { return &_params; } parameters *getParamHandle() { return &_params; }
// set vehicle landed status data // set vehicle landed status data
void set_in_air_status(bool in_air) { _control_status.flags.in_air = in_air; } void set_in_air_status(bool in_air)
{
if (!in_air) {
_time_last_on_ground_us = _time_last_imu;
} else {
_time_last_in_air = _time_last_imu;
}
_control_status.flags.in_air = in_air;
}
// return true if the attitude is usable // return true if the attitude is usable
bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; } bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
@ -320,6 +329,8 @@ protected:
// [1] Vertical position drift rate (m/s) // [1] Vertical position drift rate (m/s)
// [2] Filtered horizontal velocity (m/s) // [2] Filtered horizontal velocity (m/s)
uint64_t _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds uint64_t _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds
uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec)
uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec)
bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval
// data buffer instances // data buffer instances