mirror of https://github.com/ArduPilot/ardupilot
sitl: support MAVLink 1.0 builds
This commit is contained in:
parent
cbecb3eff9
commit
532ac607b2
|
@ -344,6 +344,16 @@ void sitl_simstate_send(uint8_t chan)
|
||||||
{
|
{
|
||||||
double p, q, r;
|
double p, q, r;
|
||||||
float yaw;
|
float yaw;
|
||||||
|
extern void mavlink_simstate_send(uint8_t chan,
|
||||||
|
float roll,
|
||||||
|
float pitch,
|
||||||
|
float yaw,
|
||||||
|
float xAcc,
|
||||||
|
float yAcc,
|
||||||
|
float zAcc,
|
||||||
|
float p,
|
||||||
|
float q,
|
||||||
|
float r);
|
||||||
|
|
||||||
// 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
|
||||||
|
@ -357,12 +367,12 @@ void sitl_simstate_send(uint8_t chan)
|
||||||
yaw -= 360;
|
yaw -= 360;
|
||||||
}
|
}
|
||||||
|
|
||||||
mavlink_msg_simstate_send((mavlink_channel_t)chan,
|
mavlink_simstate_send(chan,
|
||||||
ToRad(sim_state.rollDeg),
|
ToRad(sim_state.rollDeg),
|
||||||
ToRad(sim_state.pitchDeg),
|
ToRad(sim_state.pitchDeg),
|
||||||
ToRad(yaw),
|
ToRad(yaw),
|
||||||
sim_state.xAccel,
|
sim_state.xAccel,
|
||||||
sim_state.yAccel,
|
sim_state.yAccel,
|
||||||
sim_state.zAccel,
|
sim_state.zAccel,
|
||||||
p, q, r);
|
p, q, r);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue