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

SITL: make the yaw match the APM conventions

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-07 17:20:47 +11:00
parent d1713bd2fb
commit 97faa47ba6
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/Desktop/support/sitl.cpp
View File

@ -324,6 +324,7 @@ void sitl_setup(void)
void sitl_simstate_send(uint8_t chan) void sitl_simstate_send(uint8_t chan)
{ {
double p, q, r; double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the // we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame // raw_imu message, which is in body frame
@ -331,10 +332,16 @@ void sitl_simstate_send(uint8_t chan)
sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate, sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate,
&p, &q, &r); &p, &q, &r);
// convert to same conventions as DCM
yaw = sim_state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
mavlink_msg_simstate_send((mavlink_channel_t)chan, mavlink_msg_simstate_send((mavlink_channel_t)chan,
ToRad(sim_state.rollDeg), ToRad(sim_state.rollDeg),
ToRad(sim_state.pitchDeg), ToRad(sim_state.pitchDeg),
ToRad(sim_state.yawDeg), ToRad(yaw),
sim_state.xAccel, sim_state.xAccel,
sim_state.yAccel, sim_state.yAccel,
sim_state.zAccel, sim_state.zAccel,