From be01fcfdfdd359244256292fdd2e48aba21c66df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Sep 2023 14:09:21 +1000 Subject: [PATCH] SITL: use mavlink_XXX_encode_status() this avoids clobbering sequence numbers and fixes race conditions --- libraries/SITL/SIM_ADSB.cpp | 38 +++++++++---------------------- libraries/SITL/SIM_Gimbal.cpp | 37 ++++++++++-------------------- libraries/SITL/SIM_Morse.cpp | 7 ++---- libraries/SITL/SIM_RF_MAVLink.cpp | 9 ++++---- libraries/SITL/SIM_RF_MAVLink.h | 2 +- libraries/SITL/SIM_Ship.cpp | 9 +++++--- libraries/SITL/SIM_Ship.h | 1 + libraries/SITL/SIM_Vicon.cpp | 15 ++++++++---- libraries/SITL/SIM_Vicon.h | 2 ++ libraries/SITL/SIM_config.h | 2 +- 10 files changed, 51 insertions(+), 71 deletions(-) diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp index 82cfe6173e..4e2d4be709 100644 --- a/libraries/SITL/SIM_ADSB.cpp +++ b/libraries/SITL/SIM_ADSB.cpp @@ -171,17 +171,10 @@ void ADSB::send_report(const class Aircraft &aircraft) heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; - /* - save and restore sequence number for chan0, as it is used by - generated encode functions - */ - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - len = mavlink_msg_heartbeat_encode(vehicle_system_id, - vehicle_component_id, - &msg, &heartbeat); - chan0_status->current_tx_seq = saved_seq; + len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, + vehicle_component_id, + &mavlink.status, + &msg, &heartbeat); write_to_autopilot((char*)&msg.magic, len); @@ -235,14 +228,11 @@ void ADSB::send_report(const class Aircraft &aircraft) adsb_vehicle.squawk = 1200; - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id, + len = mavlink_msg_adsb_vehicle_encode_status(vehicle_system_id, MAV_COMP_ID_ADSB, + &mavlink.status, &msg, &adsb_vehicle); - chan0_status->current_tx_seq = saved_seq; - + uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { @@ -255,17 +245,11 @@ void ADSB::send_report(const class Aircraft &aircraft) if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { last_tx_report_ms = now; - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - uint8_t saved_flags = chan0_status->flags; - chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - chan0_status->current_tx_seq = mavlink.seq; const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK}; - len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id, - MAV_COMP_ID_ADSB, - &msg, &health_report); - chan0_status->current_tx_seq = saved_seq; - chan0_status->flags = saved_flags; + len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode_status(vehicle_system_id, + MAV_COMP_ID_ADSB, + &mavlink.status, + &msg, &health_report); uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index 0fc8607f48..a1997dd00f 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -236,13 +236,10 @@ void Gimbal::param_send(const struct gimbal_param *p) param_value.param_index = 0; param_value.param_type = MAV_PARAM_TYPE_REAL32; - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - uint16_t len = mavlink_msg_param_value_encode(vehicle_system_id, - vehicle_component_id, - &msg, ¶m_value); - chan0_status->current_tx_seq = saved_seq; + uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id, + vehicle_component_id, + &mavlink.status, + &msg, ¶m_value); uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); @@ -366,17 +363,10 @@ void Gimbal::send_report(void) heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; - /* - save and restore sequence number for chan0, as it is used by - generated encode functions - */ - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - len = mavlink_msg_heartbeat_encode(vehicle_system_id, - vehicle_component_id, - &msg, &heartbeat); - chan0_status->current_tx_seq = saved_seq; + len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, + vehicle_component_id, + &mavlink.status, + &msg, &heartbeat); mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; @@ -403,13 +393,10 @@ void Gimbal::send_report(void) gimbal_report.joint_el = joint_angles.y; gimbal_report.joint_az = joint_angles.z; - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - len = mavlink_msg_gimbal_report_encode(vehicle_system_id, - vehicle_component_id, - &msg, &gimbal_report); - chan0_status->current_tx_seq = saved_seq; + len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id, + vehicle_component_id, + &mavlink.status, + &msg, &gimbal_report); uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index d4303b87ee..81ba8ce349 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -656,14 +656,11 @@ void Morse::send_report(void) } mavlink_message_t msg; - mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); - uint8_t saved_seq = chan0_status->current_tx_seq; - chan0_status->current_tx_seq = mavlink.seq; - uint16_t len = mavlink_msg_obstacle_distance_encode( + uint16_t len = mavlink_msg_obstacle_distance_encode_status( mavlink_system.sysid, 13, + &mavlink.status, &msg, &packet); - chan0_status->current_tx_seq = saved_seq; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); diff --git a/libraries/SITL/SIM_RF_MAVLink.cpp b/libraries/SITL/SIM_RF_MAVLink.cpp index f7384bb63a..651eb66d5a 100644 --- a/libraries/SITL/SIM_RF_MAVLink.cpp +++ b/libraries/SITL/SIM_RF_MAVLink.cpp @@ -43,10 +43,11 @@ uint32_t RF_MAVLink::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t bu .vertical_fov = 0, // 0 is unknown vertical fov .quaternion = {0,0,0,0} // unknown/unused quat }; - const uint16_t len = mavlink_msg_distance_sensor_encode(system_id, - component_id, - &msg, - &distance_sensor); + const uint16_t len = mavlink_msg_distance_sensor_encode_status(system_id, + component_id, + &mav_status, + &msg, + &distance_sensor); if (len > buflen) { AP_HAL::panic("Insufficient buffer passed in"); } diff --git a/libraries/SITL/SIM_RF_MAVLink.h b/libraries/SITL/SIM_RF_MAVLink.h index cc5c284cb4..23b04bdaf0 100644 --- a/libraries/SITL/SIM_RF_MAVLink.h +++ b/libraries/SITL/SIM_RF_MAVLink.h @@ -39,7 +39,7 @@ public: uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; private: - + mavlink_status_t mav_status; }; } diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index 9e095f146e..f0bf379a13 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -178,9 +178,10 @@ void ShipSim::send_report(void) 0, 0}; mavlink_message_t msg; - mavlink_msg_heartbeat_encode( + mavlink_msg_heartbeat_encode_status( sys_id.get(), component_id, + &mav_status, &msg, &heartbeat); uint8_t buf[300]; @@ -221,9 +222,10 @@ void ShipSim::send_report(void) uint16_t(ship.heading_deg*100) }; mavlink_message_t msg; - mavlink_msg_global_position_int_encode( + mavlink_msg_global_position_int_encode_status( sys_id, component_id, + &mav_status, &msg, &global_position_int); uint8_t buf[300]; @@ -240,9 +242,10 @@ void ShipSim::send_report(void) 0, 0, ship.yaw_rate }; mavlink_message_t msg; - mavlink_msg_attitude_encode( + mavlink_msg_attitude_encode_status( sys_id, component_id, + &mav_status, &msg, &attitude); uint8_t buf[300]; diff --git a/libraries/SITL/SIM_Ship.h b/libraries/SITL/SIM_Ship.h index a9c4449b0a..cbb91ccba4 100644 --- a/libraries/SITL/SIM_Ship.h +++ b/libraries/SITL/SIM_Ship.h @@ -85,6 +85,7 @@ private: SocketAPM mav_socket { false }; bool mavlink_connected; + mavlink_status_t mav_status; void send_report(void); }; diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp index dc7bb50422..b54374baf8 100644 --- a/libraries/SITL/SIM_Vicon.cpp +++ b/libraries/SITL/SIM_Vicon.cpp @@ -164,9 +164,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc, pitch, yaw }; - mavlink_msg_vision_position_estimate_encode( + mavlink_msg_vision_position_estimate_encode_status( system_id, component_id, + &mav_status, &msg_buf[msg_buf_index].obs_msg, &vision_position_estimate ); @@ -184,9 +185,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc, pitch, yaw }; - mavlink_msg_vicon_position_estimate_encode( + mavlink_msg_vicon_position_estimate_encode_status( system_id, component_id, + &mav_status, &msg_buf[msg_buf_index].obs_msg, &vicon_position_estimate); msg_buf[msg_buf_index].time_send_us = time_send_us; @@ -200,9 +202,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc, vel_corrected.y, vel_corrected.z }; - mavlink_msg_vision_speed_estimate_encode( + mavlink_msg_vision_speed_estimate_encode_status( system_id, component_id, + &mav_status, &msg_buf[msg_buf_index].obs_msg, &vicon_speed_estimate ); @@ -232,9 +235,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc, 0, MAV_ESTIMATOR_TYPE_VIO }; - mavlink_msg_odometry_encode( + mavlink_msg_odometry_encode_status( system_id, component_id, + &mav_status, &msg_buf[msg_buf_index].obs_msg, &odometry); msg_buf[msg_buf_index].time_send_us = time_send_us; @@ -270,9 +274,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc, }, {pos_delta.x, pos_delta.y, pos_delta.z} }; - mavlink_msg_vision_position_delta_encode( + mavlink_msg_vision_position_delta_encode_status( system_id, component_id, + &mav_status, &msg_buf[msg_buf_index].obs_msg, &vision_position_delta); msg_buf[msg_buf_index].time_send_us = time_send_us; diff --git a/libraries/SITL/SIM_Vicon.h b/libraries/SITL/SIM_Vicon.h index e7eb27ca42..5cb9aadce4 100644 --- a/libraries/SITL/SIM_Vicon.h +++ b/libraries/SITL/SIM_Vicon.h @@ -77,6 +77,8 @@ private: // position delta message Quaternion _attitude_prev; // Rotation to previous MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED Vector3d _position_prev; // previous position from origin (m) MAV_FRAME_LOCAL_NED + + mavlink_status_t mav_status; }; } diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index bf0ebe588b..92521f0b7f 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -3,7 +3,7 @@ #include #ifndef HAL_SIM_ADSB_ENABLED -#define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && !defined(HAL_BUILD_AP_PERIPH) +#define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif #ifndef HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED