Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: only recalculate ahrs rotation matrix if necessary

Browse Source
This commit is contained in:
Mark Whitehorn 2019-12-11 17:11:03 -07:00 committed by Andrew Tridgell
parent ab33679124
commit 76b985fc9c
3 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/SITL/SIM_Aircraft.cpp
View File

@ -73,6 +73,12 @@ Aircraft::Aircraft(const char *frame_str) :
// allow for orientation settings, such as with tailsitters
enum ap_var_type ptype;
ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
ahrs_rotation_inv.from_rotation(imu_rotation);
ahrs_rotation_inv.transpose();
last_imu_rotation = imu_rotation;
terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_"));
}
@ -371,11 +377,14 @@ 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) {
ahrs_rotation_inv.from_rotation(imu_rotation);
ahrs_rotation_inv.transpose();
last_imu_rotation = imu_rotation;
}
if (imu_rotation != ROTATION_NONE) {
Matrix3f m = dcm;
Matrix3f rot;
rot.from_rotation(imu_rotation);
m = m * rot.transposed();
m = m * ahrs_rotation_inv;
m.to_euler(&r, &p, &y);
fdm.rollDeg = degrees(r);

2
libraries/SITL/SIM_Aircraft.h
View File

@ -193,6 +193,8 @@ protected:
// allow for AHRS_ORIENTATION
AP_Int8 *ahrs_orientation;
enum Rotation last_imu_rotation;
Matrix3f ahrs_rotation_inv;
enum GroundBehaviour {
GROUND_BEHAVIOR_NONE = 0,

10
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -460,14 +460,8 @@ void FlightAxis::update(const struct sitl_input &input)
Vector3f airspeed_3d_ef = m_wind_ef + velocity_ef;
Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
if (imu_rotation != ROTATION_NONE) {
Matrix3f rot;
rot.from_rotation(imu_rotation);
airspeed3d = airspeed3d * rot.transposed();
}
if (last_imu_rotation != ROTATION_NONE) {
airspeed3d = airspeed3d * ahrs_rotation_inv;
}
airspeed_pitot = MAX(airspeed3d.x,0);