Browse Source

EKF: Add a fixed wing mode with setter function

master
Paul Riseborough 8 years ago committed by Lorenz Meier
parent
commit
d446f66105
  1. 1
      EKF/common.h
  2. 12
      EKF/control.cpp
  3. 3
      EKF/estimator_interface.h

1
EKF/common.h

@ -412,6 +412,7 @@ union filter_control_status_u { @@ -412,6 +412,7 @@ union filter_control_status_u {
uint32_t ev_hgt : 1; // 14 - true when height data from external vision measurements is being fused
uint32_t fuse_beta : 1; // 15 - true when synthetic sideslip measurements are being fused
uint32_t update_mag_states_only : 1; // 16 - true when only the magnetometer states are updated by the magnetometer
uint32_t fixed_wing : 1; // 17 - true when the vehicle is operating as a fixed wing vehicle
} flags;
uint32_t value;
};

12
EKF/control.cpp

@ -1050,14 +1050,12 @@ void Ekf::controlMagFusion() @@ -1050,14 +1050,12 @@ void Ekf::controlMagFusion()
if (!_control_status.flags.mag_3D) {
if (!_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 we are doing wind estimation and using the zero sideslip assumotion, then it is reasonable to assume the plane is flying forward
bool zeroSideslipValid = _control_status.flags.fuse_beta;
if (zeroSideslipValid) {
_flt_mag_align_complete = _control_status.flags.yaw_align = realignYawGPS();
if (_control_status.flags.fixed_wing) {
_control_status.flags.yaw_align = realignYawGPS();
_flt_mag_align_complete = _control_status.flags.yaw_align;
} else {
_flt_mag_align_complete = _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_flt_mag_align_complete = _control_status.flags.yaw_align;
}
} else {
// reset the mag field covariances

3
EKF/estimator_interface.h

@ -181,6 +181,9 @@ public: @@ -181,6 +181,9 @@ public:
// set vehicle landed status data
void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
// set vehicle is fixed wing status
void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
// set flag if synthetic sideslip measurement should be fused
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = fuse_beta;}

Loading…
Cancel
Save