/* 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 SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
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(state.rollDeg, state.pitchDeg,
state.rollRate, state.pitchRate, state.yawRate,
&p, &q, &r);
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
