Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add support for SIM_STATE msg

This commit is contained in:
Pierre Kancir 2021-01-15 08:54:15 +01:00 committed by Andrew Tridgell
parent 8eb670d685
commit c1f1208c89
2 changed files with 35 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/SITL/SITL.cpp
View File

@ -371,7 +371,7 @@ const AP_Param::GroupInfo SITL::var_sfml_joystick[] = {
#endif //SFML_JOYSTICK
/* report SITL state via MAVLink */
/* report SITL state via MAVLink SIMSTATE*/
void SITL::simstate_send(mavlink_channel_t chan)
{
float yaw;
@ -396,6 +396,39 @@ void SITL::simstate_send(mavlink_channel_t chan)
state.longitude*1.0e7);
}
/* report SITL state via MAVLink SIM_STATE */
void SITL::sim_state_send(mavlink_channel_t chan)
{
// convert to same conventions as DCM
float yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
mavlink_msg_sim_state_send(chan,
state.quaternion.q1,
state.quaternion.q2,
state.quaternion.q3,
state.quaternion.q4,
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,
(float)state.altitude,
0.0,
0.0,
state.speedN,
state.speedE,
state.speedD);
}
/* report SITL state to AP_Logger */
void SITL::Log_Write_SIMSTATE()
{

1
libraries/SITL/SITL.h
View File

@ -387,6 +387,7 @@ public:
time_t start_time_UTC;
void simstate_send(mavlink_channel_t chan);
void sim_state_send(mavlink_channel_t chan);
void Log_Write_SIMSTATE();