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

SITL: use mavlink_XXX_encode_status() 

this avoids clobbering sequence numbers and fixes race conditions
This commit is contained in:
Andrew Tridgell 2023-09-29 14:09:21 +10:00
parent e7a94df89a
commit be01fcfdfd
10 changed files with 51 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/SITL/SIM_ADSB.cpp
View File

@ -171,17 +171,10 @@ void ADSB::send_report(const class Aircraft &aircraft)
heartbeat.mavlink_version = 0; heartbeat.mavlink_version = 0;
heartbeat.custom_mode = 0; heartbeat.custom_mode = 0;
/* len = mavlink_msg_heartbeat_encode_status(vehicle_system_id,
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, vehicle_component_id,
&mavlink.status,
&msg, &heartbeat); &msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq;
write_to_autopilot((char*)&msg.magic, len); write_to_autopilot((char*)&msg.magic, len);
@ -235,13 +228,10 @@ void ADSB::send_report(const class Aircraft &aircraft)
adsb_vehicle.squawk = 1200; adsb_vehicle.squawk = 1200;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); len = mavlink_msg_adsb_vehicle_encode_status(vehicle_system_id,
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,
MAV_COMP_ID_ADSB, MAV_COMP_ID_ADSB,
&mavlink.status,
&msg, &adsb_vehicle); &msg, &adsb_vehicle);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len]; uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg); len = mavlink_msg_to_send_buffer(msgbuf, &msg);
@ -255,17 +245,11 @@ void ADSB::send_report(const class Aircraft &aircraft)
if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) {
last_tx_report_ms = now; 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}; 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, len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode_status(vehicle_system_id,
MAV_COMP_ID_ADSB, MAV_COMP_ID_ADSB,
&mavlink.status,
&msg, &health_report); &msg, &health_report);
chan0_status->current_tx_seq = saved_seq;
chan0_status->flags = saved_flags;
uint8_t msgbuf[len]; uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg); len = mavlink_msg_to_send_buffer(msgbuf, &msg);

25
libraries/SITL/SIM_Gimbal.cpp
View File

@ -236,13 +236,10 @@ void Gimbal::param_send(const struct gimbal_param *p)
param_value.param_index = 0; param_value.param_index = 0;
param_value.param_type = MAV_PARAM_TYPE_REAL32; param_value.param_type = MAV_PARAM_TYPE_REAL32;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id,
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, vehicle_component_id,
&mavlink.status,
&msg, &param_value); &msg, &param_value);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len]; uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg); len = mavlink_msg_to_send_buffer(msgbuf, &msg);
@ -366,17 +363,10 @@ void Gimbal::send_report(void)
heartbeat.mavlink_version = 0; heartbeat.mavlink_version = 0;
heartbeat.custom_mode = 0; heartbeat.custom_mode = 0;
/* len = mavlink_msg_heartbeat_encode_status(vehicle_system_id,
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, vehicle_component_id,
&mavlink.status,
&msg, &heartbeat); &msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len); mav_socket.send(&msg.magic, len);
last_heartbeat_ms = now; last_heartbeat_ms = now;
@ -403,13 +393,10 @@ void Gimbal::send_report(void)
gimbal_report.joint_el = joint_angles.y; gimbal_report.joint_el = joint_angles.y;
gimbal_report.joint_az = joint_angles.z; gimbal_report.joint_az = joint_angles.z;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id,
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, vehicle_component_id,
&mavlink.status,
&msg, &gimbal_report); &msg, &gimbal_report);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len]; uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg); len = mavlink_msg_to_send_buffer(msgbuf, &msg);

7
libraries/SITL/SIM_Morse.cpp
View File

@ -656,14 +656,11 @@ void Morse::send_report(void)
} }
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint16_t len = mavlink_msg_obstacle_distance_encode_status(
uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq;
uint16_t len = mavlink_msg_obstacle_distance_encode(
mavlink_system.sysid, mavlink_system.sysid,
13, 13,
&mavlink.status,
&msg, &packet); &msg, &packet);
chan0_status->current_tx_seq = saved_seq;
uint8_t msgbuf[len]; uint8_t msgbuf[len];
len = mavlink_msg_to_send_buffer(msgbuf, &msg); len = mavlink_msg_to_send_buffer(msgbuf, &msg);

3
libraries/SITL/SIM_RF_MAVLink.cpp
View File

@ -43,8 +43,9 @@ 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 .vertical_fov = 0, // 0 is unknown vertical fov
.quaternion = {0,0,0,0} // unknown/unused quat .quaternion = {0,0,0,0} // unknown/unused quat
}; };
const uint16_t len = mavlink_msg_distance_sensor_encode(system_id, const uint16_t len = mavlink_msg_distance_sensor_encode_status(system_id,
component_id, component_id,
&mav_status,
&msg, &msg,
&distance_sensor); &distance_sensor);
if (len > buflen) { if (len > buflen) {

2
libraries/SITL/SIM_RF_MAVLink.h
View File

@ -39,7 +39,7 @@ public:
uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override; uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
private: private:
mavlink_status_t mav_status;
}; };
} }

9
libraries/SITL/SIM_Ship.cpp
View File

@ -178,9 +178,10 @@ void ShipSim::send_report(void)
0, 0,
0}; 0};
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_heartbeat_encode( mavlink_msg_heartbeat_encode_status(
sys_id.get(), sys_id.get(),
component_id, component_id,
&mav_status,
&msg, &msg,
&heartbeat); &heartbeat);
uint8_t buf[300]; uint8_t buf[300];
@ -221,9 +222,10 @@ void ShipSim::send_report(void)
uint16_t(ship.heading_deg*100) uint16_t(ship.heading_deg*100)
}; };
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_global_position_int_encode( mavlink_msg_global_position_int_encode_status(
sys_id, sys_id,
component_id, component_id,
&mav_status,
&msg, &msg,
&global_position_int); &global_position_int);
uint8_t buf[300]; uint8_t buf[300];
@ -240,9 +242,10 @@ void ShipSim::send_report(void)
0, 0, ship.yaw_rate 0, 0, ship.yaw_rate
}; };
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_attitude_encode( mavlink_msg_attitude_encode_status(
sys_id, sys_id,
component_id, component_id,
&mav_status,
&msg, &msg,
&attitude); &attitude);
uint8_t buf[300]; uint8_t buf[300];

1
libraries/SITL/SIM_Ship.h
View File

@ -85,6 +85,7 @@ private:
SocketAPM mav_socket { false }; SocketAPM mav_socket { false };
bool mavlink_connected; bool mavlink_connected;
mavlink_status_t mav_status;
void send_report(void); void send_report(void);
}; };

15
libraries/SITL/SIM_Vicon.cpp
View File

@ -164,9 +164,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
pitch, pitch,
yaw yaw
}; };
mavlink_msg_vision_position_estimate_encode( mavlink_msg_vision_position_estimate_encode_status(
system_id, system_id,
component_id, component_id,
&mav_status,
&msg_buf[msg_buf_index].obs_msg, &msg_buf[msg_buf_index].obs_msg,
&vision_position_estimate &vision_position_estimate
); );
@ -184,9 +185,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
pitch, pitch,
yaw yaw
}; };
mavlink_msg_vicon_position_estimate_encode( mavlink_msg_vicon_position_estimate_encode_status(
system_id, system_id,
component_id, component_id,
&mav_status,
&msg_buf[msg_buf_index].obs_msg, &msg_buf[msg_buf_index].obs_msg,
&vicon_position_estimate); &vicon_position_estimate);
msg_buf[msg_buf_index].time_send_us = time_send_us; 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.y,
vel_corrected.z vel_corrected.z
}; };
mavlink_msg_vision_speed_estimate_encode( mavlink_msg_vision_speed_estimate_encode_status(
system_id, system_id,
component_id, component_id,
&mav_status,
&msg_buf[msg_buf_index].obs_msg, &msg_buf[msg_buf_index].obs_msg,
&vicon_speed_estimate &vicon_speed_estimate
); );
@ -232,9 +235,10 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
0, 0,
MAV_ESTIMATOR_TYPE_VIO MAV_ESTIMATOR_TYPE_VIO
}; };
mavlink_msg_odometry_encode( mavlink_msg_odometry_encode_status(
system_id, system_id,
component_id, component_id,
&mav_status,
&msg_buf[msg_buf_index].obs_msg, &msg_buf[msg_buf_index].obs_msg,
&odometry); &odometry);
msg_buf[msg_buf_index].time_send_us = time_send_us; 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} {pos_delta.x, pos_delta.y, pos_delta.z}
}; };
mavlink_msg_vision_position_delta_encode( mavlink_msg_vision_position_delta_encode_status(
system_id, system_id,
component_id, component_id,
&mav_status,
&msg_buf[msg_buf_index].obs_msg, &msg_buf[msg_buf_index].obs_msg,
&vision_position_delta); &vision_position_delta);
msg_buf[msg_buf_index].time_send_us = time_send_us; msg_buf[msg_buf_index].time_send_us = time_send_us;

2
libraries/SITL/SIM_Vicon.h
View File

@ -77,6 +77,8 @@ private:
// position delta message // position delta message
Quaternion _attitude_prev; // Rotation to previous MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED 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 Vector3d _position_prev; // previous position from origin (m) MAV_FRAME_LOCAL_NED
mavlink_status_t mav_status;
}; };
} }

2
libraries/SITL/SIM_config.h
View File

@ -3,7 +3,7 @@
#include <AP_HAL/AP_HAL_Boards.h> #include <AP_HAL/AP_HAL_Boards.h>
#ifndef HAL_SIM_ADSB_ENABLED #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 #endif
#ifndef HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED #ifndef HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED