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

SITL: support pitch90 and pitch270 in FlightAxis

Browse Source 
used for flying tailsitters
This commit is contained in:
Andrew Tridgell 2017-02-10 16:28:09 +11:00
parent 1a74b7fc0c
commit 8bd1fc63d5
2 changed files with 37 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -50,6 +50,28 @@ FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
// FlightAxis sensor data is not good enough for EKF. Use fake EKF by default
AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10);
AP_Param::set_default_by_name("INS_GYR_CAL", 0);
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);
}
}
/*
@ -284,9 +306,11 @@ void FlightAxis::update(const struct sitl_input &input)
state.m_orientationQuaternion_X,
-state.m_orientationQuaternion_Z);
quat.rotation_matrix(dcm);
gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
-radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
state.m_velocityWorldV_MPS,
state.m_velocityWorldW_MPS);
@ -297,6 +321,15 @@ void FlightAxis::update(const struct sitl_input &input)
accel_body(state.m_accelerationBodyAX_MPS2,
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;

4
libraries/SITL/SIM_FlightAxis.h
View File

@ -166,7 +166,9 @@ private:
double last_frame_count_s = 0;
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;
};