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

View File

@ -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);

View File

@ -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, &param_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, &param_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);

View File

@ -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);

View File

@ -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");
}

View File

@ -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;
};
}

View File

@ -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];

View File

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

View File

@ -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;

View File

@ -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;
};
}

View File

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