Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: rotate quaternions into vehicle body frame

Browse Source 
DCM was having it both ways depending on whether it was
primary/secondary.

These are mostly uses for reporting, in which case you would expect the
quaternion to match the eulers.
This commit is contained in:
Peter Barker 2021-07-23 15:42:45 +10:00 committed by Andrew Tridgell
parent 7929122f49
commit f557d41145
1 changed files with 43 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -900,36 +900,50 @@ bool AP_AHRS::use_compass(void)
// return the quaternion defining the rotation from NED to XYZ (body) axes // return the quaternion defining the rotation from NED to XYZ (body) axes
bool AP_AHRS::get_quaternion(Quaternion &quat) const bool AP_AHRS::get_quaternion(Quaternion &quat) const
{ {
// backends always return in autopilot XYZ frame; rotate result
// according to trim
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
return AP_AHRS_DCM::get_quaternion(quat); if (!AP_AHRS_DCM::get_quaternion(quat)) {
return false;
}
break;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
if (!_ekf2_started) {
return false;
}
EKF2.getQuaternion(-1, quat); EKF2.getQuaternion(-1, quat);
return _ekf2_started; break;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE: case EKFType::THREE:
EKF3.getQuaternion(-1, quat); if (!_ekf3_started) {
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; return false;
} }
EKF3.getQuaternion(-1, quat);
break;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL: {
if (!_sitl) {
return false;
}
const struct SITL::sitl_fdm &fdm = _sitl->state;
quat = fdm.quaternion;
break;
}
#endif #endif
#if HAL_EXTERNAL_AHRS_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL: case EKFType::EXTERNAL:
// we assume the external AHRS isn't trimmed with the autopilot!
return AP::externalAHRS().get_quaternion(quat); return AP::externalAHRS().get_quaternion(quat);
#endif #endif
} }
// since there is no default case above, this is unreachable
return false; quat.rotate(-_trim.get());
return true;
} }
// return secondary attitude solution if available, as eulers in radians // return secondary attitude solution if available, as eulers in radians
@ -997,21 +1011,29 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
case EKFType::NONE: case EKFType::NONE:
// DCM is secondary // DCM is secondary
quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned()); if (!AP_AHRS_DCM::get_quaternion(quat)) {
return true; return false;
}
break;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
// EKF2 is secondary // EKF2 is secondary
if (!_ekf2_started) {
return false;
}
EKF2.getQuaternion(-1, quat); EKF2.getQuaternion(-1, quat);
return _ekf2_started; break;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE: case EKFType::THREE:
// EKF3 is secondary // EKF3 is secondary
if (!_ekf3_started) {
return false;
}
EKF3.getQuaternion(-1, quat); EKF3.getQuaternion(-1, quat);
return _ekf3_started; break;
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -1027,8 +1049,9 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
#endif #endif
} }
// since there is no default case above, this is unreachable quat.rotate(-_trim.get());
return false;
return true;
} }
// return secondary position solution if available // return secondary position solution if available