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

SITL: add support for AHRS_ORIENTATION=ROTATION_CUSTOM

Browse Source
This commit is contained in:
Mark Whitehorn 2020-01-13 10:47:52 -07:00 committed by Andrew Tridgell
parent 7d3d664ddf
commit db1c7d9bdd
4 changed files with 32 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/SITL/SIM_Aircraft.cpp
View File

@ -75,9 +75,10 @@ Aircraft::Aircraft(const char *frame_str) :
enum ap_var_type ptype; enum ap_var_type ptype;
ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype); ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
// ahrs_orientation->get() returns ROTATION_NONE here, regardless of the actual value
enum Rotation imu_rotation = ahrs_orientation?(enum Rotation)ahrs_orientation->get():ROTATION_NONE; enum Rotation imu_rotation = ahrs_orientation?(enum Rotation)ahrs_orientation->get():ROTATION_NONE;
ahrs_rotation_inv.from_rotation(imu_rotation); sitl->ahrs_rotation.from_rotation(imu_rotation);
ahrs_rotation_inv.transpose(); sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed();
last_imu_rotation = imu_rotation; last_imu_rotation = imu_rotation;
terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_")); terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_"));
@ -389,15 +390,31 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
if (ahrs_orientation != nullptr) { if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
if (imu_rotation != last_imu_rotation) { if (imu_rotation != last_imu_rotation) {
ahrs_rotation_inv.from_rotation(imu_rotation); // Surprisingly, Matrix3<T>::from_rotation(ROTATION_CUSTOM) is the identity matrix
ahrs_rotation_inv.transpose(); // so we must deal with that here
last_imu_rotation = imu_rotation; 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;
}
} }
if (imu_rotation != ROTATION_NONE) { if (imu_rotation != ROTATION_NONE) {
Matrix3f m = dcm; Matrix3f m = dcm;
m = m * ahrs_rotation_inv; m = m * sitl->ahrs_rotation_inv;
m.to_euler(&r, &p, &y); m.to_euler(&r, &p, &y);
fdm.rollDeg = degrees(r); fdm.rollDeg = degrees(r);

4
libraries/SITL/SIM_Aircraft.h
View File

@ -205,7 +205,9 @@ protected:
// allow for AHRS_ORIENTATION // allow for AHRS_ORIENTATION
AP_Int8 *ahrs_orientation; AP_Int8 *ahrs_orientation;
enum Rotation last_imu_rotation; enum Rotation last_imu_rotation;
Matrix3f ahrs_rotation_inv; AP_Float* custom_roll;
AP_Float* custom_pitch;
AP_Float* custom_yaw;
enum GroundBehaviour { enum GroundBehaviour {
GROUND_BEHAVIOR_NONE = 0, GROUND_BEHAVIOR_NONE = 0,

2
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -462,7 +462,7 @@ void FlightAxis::update(const struct sitl_input &input)
Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef); Vector3f airspeed3d = dcm.mul_transpose(airspeed_3d_ef);
if (last_imu_rotation != ROTATION_NONE) { if (last_imu_rotation != ROTATION_NONE) {
airspeed3d = airspeed3d * ahrs_rotation_inv; airspeed3d = airspeed3d * sitl->ahrs_rotation_inv;
} }
airspeed_pitot = MAX(airspeed3d.x,0); airspeed_pitot = MAX(airspeed3d.x,0);

4
libraries/SITL/SITL.h
View File

@ -124,6 +124,10 @@ public:
static const struct AP_Param::GroupInfo var_info2[]; static const struct AP_Param::GroupInfo var_info2[];
static const struct AP_Param::GroupInfo var_info3[]; static const struct AP_Param::GroupInfo var_info3[];
// Board Orientation (and inverse)
Matrix3f ahrs_rotation;
Matrix3f ahrs_rotation_inv;
// noise levels for simulated sensors // noise levels for simulated sensors
AP_Float baro_noise; // in metres AP_Float baro_noise; // in metres
AP_Float baro_drift; // in metres per second AP_Float baro_drift; // in metres per second