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

SITL: support AHRS_ORIENTATION in SITL

Browse Source 
this allows for tailsitters in RealFlight
This commit is contained in:
Andrew Tridgell 2017-11-23 14:26:01 +11:00
parent ebcffcb0ac
commit 917d9842c4
4 changed files with 21 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/SITL/SIM_Aircraft.cpp
View File

@ -73,11 +73,10 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
last_wall_time_us = get_wall_time_us();
frame_counter = 0;
// support rotated IMUs for testing
if (strstr(frame_str, "-roll180")) {
imu_rotation = ROTATION_ROLL_180;
}
// allow for orientation settings, such as with tailsitters
enum ap_var_type ptype;
ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_");
}
@ -392,18 +391,21 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.altitude = smoothing.location.alt * 1.0e-2;
}
if (imu_rotation != ROTATION_NONE) {
Vector3f accel(fdm.xAccel, fdm.yAccel, fdm.zAccel);
accel.rotate(imu_rotation);
fdm.xAccel = accel.x;
fdm.yAccel = accel.y;
fdm.zAccel = accel.z;
if (ahrs_orientation != nullptr) {
enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get();
Vector3f rgyro(fdm.rollRate, fdm.pitchRate, fdm.yawRate);
rgyro.rotate(imu_rotation);
fdm.rollRate = degrees(rgyro.x);
fdm.pitchRate = degrees(rgyro.y);
fdm.yawRate = degrees(rgyro.z);
if (imu_rotation != ROTATION_NONE) {
Matrix3f m = dcm;
Matrix3f rot;
rot.from_rotation(imu_rotation);
m = m * rot.transposed();
m.to_euler(&r, &p, &y);
fdm.rollDeg = degrees(r);
fdm.pitchDeg = degrees(p);
fdm.yawDeg = degrees(y);
fdm.quaternion.from_rotation_matrix(m);
}
}
if (last_speedup != sitl->speedup && sitl->speedup > 0) {

5
libraries/SITL/SIM_Aircraft.h
View File

@ -164,8 +164,9 @@ protected:
bool use_time_sync = true;
float last_speedup = -1.0f;
enum Rotation imu_rotation = ROTATION_NONE;
// allow for AHRS_ORIENTATION
AP_Int8 *ahrs_orientation;
enum {
GROUND_BEHAVIOR_NONE = 0,
GROUND_BEHAVIOR_NO_MOVEMENT,

30
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -87,28 +87,6 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
}
if (strstr(frame_str, "pitch270")) {
// rotate tailsitter airframes for fixed wing view
rotation = ROTATION_PITCH_270;
}
if (strstr(frame_str, "pitch90")) {
// rotate tailsitter airframes for fixed wing view
rotation = ROTATION_PITCH_90;
}
switch (rotation) {
case ROTATION_NONE:
break;
case ROTATION_PITCH_90:
att_rotation.from_euler(0, radians(90), 0);
break;
case ROTATION_PITCH_270:
att_rotation.from_euler(0, radians(270), 0);
break;
default:
AP_HAL::panic("Unsupported flightaxis rotation %u\n", (unsigned)rotation);
}
/* Create the thread that will be waiting for data from FlightAxis */
mutex = hal.util->new_semaphore();
@ -428,14 +406,6 @@ void FlightAxis::update(const struct sitl_input &input)
state.m_accelerationBodyAY_MPS2,
state.m_accelerationBodyAZ_MPS2);
if (rotation != ROTATION_NONE) {
dcm.transpose();
dcm = att_rotation * dcm;
dcm.transpose();
gyro.rotate(rotation);
accel_body.rotate(rotation);
}
// accel on the ground is nasty in realflight, and prevents helicopter disarm
if (state.m_isTouchingGround) {
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;

2
libraries/SITL/SIM_FlightAxis.h
View File

@ -176,8 +176,6 @@ private:
double last_frame_count_s;
Vector3f position_offset;
Vector3f last_velocity_ef;
Matrix3f att_rotation;
enum Rotation rotation = ROTATION_NONE;
const char *controller_ip = "127.0.0.1";
uint16_t controller_port = 18083;