Browse Source

AP_AHRS: add get_quaternion

c415-sdk
Randy Mackay 5 years ago
parent
commit
b0b78e974b
  1. 2
      libraries/AP_AHRS/AP_AHRS.h
  2. 7
      libraries/AP_AHRS/AP_AHRS_DCM.cpp
  3. 3
      libraries/AP_AHRS/AP_AHRS_DCM.h
  4. 30
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
  5. 3
      libraries/AP_AHRS/AP_AHRS_NavEKF.h

2
libraries/AP_AHRS/AP_AHRS.h

@ -392,6 +392,8 @@ public: @@ -392,6 +392,8 @@ public:
return _sin_yaw;
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;
// return secondary attitude solution if available, as eulers in radians
virtual bool get_secondary_attitude(Vector3f &eulers) const WARN_IF_UNUSED {

7
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -473,6 +473,13 @@ bool AP_AHRS_DCM::use_compass(void) @@ -473,6 +473,13 @@ bool AP_AHRS_DCM::use_compass(void)
return true;
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS_DCM::get_quaternion(Quaternion &quat) const
{
quat.from_rotation_matrix(_dcm_matrix);
return true;
}
// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate

3
libraries/AP_AHRS/AP_AHRS_DCM.h

@ -96,6 +96,9 @@ public: @@ -96,6 +96,9 @@ public:
bool use_compass() override;
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
bool set_home(const Location &loc) override WARN_IF_UNUSED;
void estimate_wind(void);

30
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -545,6 +545,36 @@ bool AP_AHRS_NavEKF::use_compass(void) @@ -545,6 +545,36 @@ bool AP_AHRS_NavEKF::use_compass(void)
return AP_AHRS_DCM::use_compass();
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_quaternion(quat);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
EKF2.getQuaternion(-1, quat);
return _ekf2_started;
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
EKF3.getQuaternion(-1, quat);
return _ekf3_started;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL:
if (_sitl) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
quat = fdm.quaternion;
return true;
} else {
return false;
}
#endif
}
// since there is no default case above, this is unreachable
return false;
}
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h

@ -115,6 +115,9 @@ public: @@ -115,6 +115,9 @@ public:
}
#endif
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
// return secondary attitude solution if available, as eulers in radians
bool get_secondary_attitude(Vector3f &eulers) const override;

Loading…
Cancel
Save