aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: remove custom rotations

Browse Source
This commit is contained in:
Iampete1 2021-11-05 16:13:20 +00:00 committed by Andrew Tridgell
parent f15c84bdac
commit b188d8fc5e
1 changed files with 3 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/SITL/SIM_Aircraft.cpp
View File

@ -418,26 +418,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
if (imu_rotation != last_imu_rotation) {
// Surprisingly, Matrix3<T>::from_rotation(ROTATION_CUSTOM) is the identity matrix
// so we must deal with that here
if (imu_rotation == ROTATION_CUSTOM) {
if ((custom_roll == nullptr) || (custom_pitch == nullptr) || (custom_yaw == nullptr)) {
enum ap_var_type ptype;
custom_roll = (AP_Float *)AP_Param::find("AHRS_CUSTOM_ROLL", &ptype);
custom_pitch = (AP_Float *)AP_Param::find("AHRS_CUSTOM_PIT", &ptype);
custom_yaw = (AP_Float *)AP_Param::find("AHRS_CUSTOM_YAW", &ptype);
}
if ((custom_roll != nullptr) && (custom_pitch != nullptr) && (custom_yaw != nullptr)) {
sitl->ahrs_rotation.from_euler(radians(*custom_roll), radians(*custom_pitch), radians(*custom_yaw));
sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
} else {
AP_HAL::panic("could not find one or more of parameters AHRS_CUSTOM_ROLL/PITCH/YAW");
}
} else {
sitl->ahrs_rotation.from_rotation(imu_rotation);
sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
last_imu_rotation = imu_rotation;
}
sitl->ahrs_rotation.from_rotation(imu_rotation);
sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
last_imu_rotation = imu_rotation;
}
if (imu_rotation != ROTATION_NONE) {
Matrix3f m = dcm;