forked from Archive/PX4-Autopilot
EKF: add flt_mag_align_complete to control_status flags
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
parent
b920910483
commit
d223680197
|
@ -448,6 +448,7 @@ union filter_control_status_u {
|
|||
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
|
||||
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
|
||||
uint32_t flt_mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed
|
||||
} flags;
|
||||
uint32_t value;
|
||||
};
|
||||
|
|
|
@ -1349,7 +1349,7 @@ void Ekf::controlMagFusion()
|
|||
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
_flt_mag_align_complete = false;
|
||||
_control_status.flags.flt_mag_align_complete = false;
|
||||
_num_bad_flight_yaw_events = 0;
|
||||
}
|
||||
|
||||
|
@ -1359,7 +1359,7 @@ void Ekf::controlMagFusion()
|
|||
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
if (!_flt_mag_align_complete) {
|
||||
if (!_control_status.flags.flt_mag_align_complete) {
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
_mag_yaw_reset_req |= (_last_on_ground_posD - _state.pos(2)) > 1.5f;
|
||||
|
@ -1368,7 +1368,7 @@ void Ekf::controlMagFusion()
|
|||
// perform a yaw reset if requested by other functions
|
||||
if (_mag_yaw_reset_req) {
|
||||
if (!_mag_use_inhibit ) {
|
||||
_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air;
|
||||
_control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air;
|
||||
}
|
||||
_mag_yaw_reset_req = false;
|
||||
}
|
||||
|
@ -1426,16 +1426,16 @@ void Ekf::controlMagFusion()
|
|||
// decide whether 3-axis magnetomer fusion can be used
|
||||
bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates
|
||||
_control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies
|
||||
(_flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height
|
||||
(_control_status.flags.flt_mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height
|
||||
((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives
|
||||
|
||||
// perform switch-over
|
||||
if (use_3D_fusion) {
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
if (!_flt_mag_align_complete) {
|
||||
if (!_control_status.flags.flt_mag_align_complete) {
|
||||
// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
|
||||
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
_flt_mag_align_complete = realignYawGPS();
|
||||
_control_status.flags.flt_mag_align_complete = realignYawGPS();
|
||||
|
||||
if (_velpos_reset_request) {
|
||||
resetVelocity();
|
||||
|
@ -1444,10 +1444,10 @@ void Ekf::controlMagFusion()
|
|||
}
|
||||
|
||||
} else {
|
||||
_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete;
|
||||
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.flt_mag_align_complete;
|
||||
|
||||
} else {
|
||||
// reset the mag field covariances
|
||||
|
@ -1462,7 +1462,7 @@ void Ekf::controlMagFusion()
|
|||
}
|
||||
|
||||
// only use one type of mag fusion at the same time
|
||||
_control_status.flags.mag_3D = _flt_mag_align_complete;
|
||||
_control_status.flags.mag_3D = _control_status.flags.flt_mag_align_complete;
|
||||
_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
|
||||
|
||||
} else {
|
||||
|
@ -1510,13 +1510,13 @@ void Ekf::controlMagFusion()
|
|||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
|
||||
if (!_control_status.flags.mag_3D || !_flt_mag_align_complete) {
|
||||
_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete;
|
||||
if (!_control_status.flags.mag_3D || !_control_status.flags.flt_mag_align_complete) {
|
||||
_control_status.flags.flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.flt_mag_align_complete;
|
||||
}
|
||||
|
||||
// use 3-axis mag fusion if reset was successful
|
||||
_control_status.flags.mag_3D = _flt_mag_align_complete;
|
||||
_control_status.flags.mag_3D = _control_status.flags.flt_mag_align_complete;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
|
||||
} else {
|
||||
|
|
|
@ -411,7 +411,6 @@ private:
|
|||
|
||||
// Variables used to control activation of post takeoff functionality
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
bool _flt_mag_align_complete{false}; ///< true when the in-flight mag field alignment has been completed
|
||||
bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd
|
||||
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
|
||||
uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
|
|
|
@ -429,7 +429,7 @@ bool Ekf::realignYawGPS()
|
|||
ECL_WARN("EKF bad yaw corrected using GPS course");
|
||||
|
||||
// declare the magnetomer as failed if a bad yaw has occurred more than once
|
||||
if (_flt_mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
if (_control_status.flags.flt_mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
ECL_WARN("EKF stopping magnetometer use");
|
||||
_control_status.flags.mag_fault = true;
|
||||
}
|
||||
|
@ -448,11 +448,11 @@ bool Ekf::realignYawGPS()
|
|||
Eulerf euler321(_state.quat_nominal);
|
||||
|
||||
// apply yaw correction
|
||||
if (!_flt_mag_align_complete) {
|
||||
if (!_control_status.flags.flt_mag_align_complete) {
|
||||
// This is our first flight aligment so we can assume that the recent change in velocity has occurred due to a
|
||||
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
|
||||
euler321(2) += courseYawError;
|
||||
_flt_mag_align_complete = true;
|
||||
_control_status.flags.flt_mag_align_complete = true;
|
||||
|
||||
} else if (_control_status.flags.wind) {
|
||||
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
|
||||
|
@ -765,7 +765,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
|
|||
void Ekf::calcMagDeclination()
|
||||
{
|
||||
// set source of magnetic declination for internal use
|
||||
if (_flt_mag_align_complete) {
|
||||
if (_control_status.flags.flt_mag_align_complete) {
|
||||
// Use value consistent with earth field state
|
||||
_mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0));
|
||||
|
||||
|
|
Loading…
Reference in New Issue