/* report SITL state via MAVLink */
void sitl_simstate_send(uint8_t chan)
{
double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(sim_state.rollDeg, sim_state.pitchDeg,
sim_state.rollRate, sim_state.pitchRate, sim_state.yawRate,
&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,
ToRad(sim_state.rollDeg),
ToRad(sim_state.pitchDeg),
ToRad(yaw),
sim_state.xAccel,
sim_state.yAccel,
sim_state.zAccel,
p, q, r);
}
// report simulator state
void GCS_MAVLINK_Plane::send_simstate() const
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
GCS_MAVLINK::send_simstate();
#elif HIL_SUPPORT
if (plane.g.hil_mode == 1) {
mavlink_msg_simstate_send(chan,
last_hil_state.roll,
last_hil_state.pitch,
last_hil_state.yaw,
last_hil_state.xacc*0.001f*GRAVITY_MSS,
last_hil_state.yacc*0.001f*GRAVITY_MSS,
last_hil_state.zacc*0.001f*GRAVITY_MSS,
last_hil_state.rollspeed,
last_hil_state.pitchspeed,
last_hil_state.yawspeed,
last_hil_state.lat,
last_hil_state.lon);
}
#endif
}
/* report SITL state via MAVLink */
void SITL::simstate_send(mavlink_channel_t chan)
{
float yaw;
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
mavlink_msg_simstate_send(chan,
ToRad(state.rollDeg),
ToRad(state.pitchDeg),
ToRad(yaw),
state.xAccel,
state.yAccel,
state.zAccel,
radians(state.rollRate),
radians(state.pitchRate),
radians(state.yawRate),
state.latitude*1.0e7,
state.longitude*1.0e7);
}
