|
|
|
@ -428,7 +428,7 @@ bool AP_AHRS_NavEKF::use_compass(void)
@@ -428,7 +428,7 @@ bool AP_AHRS_NavEKF::use_compass(void)
|
|
|
|
|
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) |
|
|
|
|
{ |
|
|
|
|
switch (active_EKF_type()) { |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
// EKF is secondary
|
|
|
|
|
EKF2.getEulerAngles(-1, eulers); |
|
|
|
|
return ekf2_started; |
|
|
|
@ -438,9 +438,9 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
@@ -438,9 +438,9 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
|
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
// DCM is secondary
|
|
|
|
|
eulers = _dcm_attitude; |
|
|
|
|
return true; |
|
|
|
|
// DCM is secondary
|
|
|
|
|
eulers = _dcm_attitude; |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -507,23 +507,23 @@ bool AP_AHRS_NavEKF::have_inertial_nav(void) const
@@ -507,23 +507,23 @@ bool AP_AHRS_NavEKF::have_inertial_nav(void) const
|
|
|
|
|
bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const |
|
|
|
|
{ |
|
|
|
|
switch (active_EKF_type()) { |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getVelNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getVelNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getVelNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getVelNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
const struct SITL::sitl_fdm &fdm = _sitl->state; |
|
|
|
|
vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
const struct SITL::sitl_fdm &fdm = _sitl->state; |
|
|
|
|
vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD); |
|
|
|
|
return true; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -532,21 +532,21 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
@@ -532,21 +532,21 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
|
|
|
|
|
bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const |
|
|
|
|
{ |
|
|
|
|
switch (active_EKF_type()) { |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getMagNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getMagNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getMagNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getMagNED(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
return false; |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
return false; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -555,22 +555,22 @@ bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
@@ -555,22 +555,22 @@ bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
|
|
|
|
|
bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const |
|
|
|
|
{ |
|
|
|
|
switch (active_EKF_type()) { |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
case EKF_TYPE_NONE: |
|
|
|
|
return false; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getMagXYZ(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE2: |
|
|
|
|
default: |
|
|
|
|
EKF2.getMagXYZ(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getMagXYZ(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
case EKF_TYPE3: |
|
|
|
|
EKF3.getMagXYZ(-1,vec); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
return false; |
|
|
|
|
#endif |
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
case EKF_TYPE_SITL: |
|
|
|
|
return false; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|