Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Sim state #16329

Merged
merged 2 commits into from Jan 18, 2021
Merged

Sim state #16329

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5a6577d
SITL: add support for SIM_STATE msg
khancyr Jan 15, 2021
d773e5a
GCS_MAVLink: send sim_state msg
khancyr Jan 15, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/GCS.h
View file
Open in desktop
Expand Up @@ -242,6 +242,7 @@ class GCS_MAVLINK
virtual void send_scaled_pressure3(); // allow sub to override this
void send_sensor_offsets();
virtual void send_simstate() const;
void send_sim_state() const;
void send_ahrs();
void send_battery2();
void send_opticalflow();
Expand Down
17 changes: 17 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Open in desktop
Expand Up @@ -791,6 +791,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS},
{ MAVLINK_MSG_ID_AHRS, MSG_AHRS},
{ MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE},
{ MAVLINK_MSG_ID_SIM_STATE, MSG_SIM_STATE},
{ MAVLINK_MSG_ID_AHRS2, MSG_AHRS2},
{ MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS},
{ MAVLINK_MSG_ID_WIND, MSG_WIND},
Expand Down Expand Up @@ -3517,6 +3518,17 @@ void GCS_MAVLINK::send_simstate() const
#endif
}

void GCS_MAVLINK::send_sim_state() const
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL *sitl = AP::sitl();
if (sitl == nullptr) {
return;
}
sitl->sim_state_send(get_chan());
#endif
}

MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_long_t &packet)
{
if (uint32_t(packet.param5) != 290876) {
Expand Down Expand Up @@ -4714,6 +4726,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_simstate();
break;

case MSG_SIM_STATE:
CHECK_PAYLOAD_SIZE(SIM_STATE);
send_sim_state();
break;

case MSG_SYS_STATUS:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_sys_status();
Expand Down
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/ap_message.h
View file
Open in desktop
Expand Up @@ -40,6 +40,7 @@ enum ap_message : uint8_t {
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_SIM_STATE,
MSG_AHRS2,
MSG_HWSTATUS,
MSG_WIND,
Expand Down
35 changes: 34 additions & 1 deletion libraries/SITL/SITL.cpp
View file
Open in desktop
Expand Up @@ -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;
Expand All @@ -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()
{
Expand Down
1 change: 1 addition & 0 deletions libraries/SITL/SITL.h
View file
Open in desktop
Expand Up @@ -387,6 +387,7 @@ class SITL {
time_t start_time_UTC;

void simstate_send(mavlink_channel_t chan);
void sim_state_send(mavlink_channel_t chan);

void Log_Write_SIMSTATE();

Expand Down