mirror of https://github.com/ArduPilot/ardupilot
SITL: use mavlink_XXX_encode_status()
this avoids clobbering sequence numbers and fixes race conditions
This commit is contained in:
parent
e7a94df89a
commit
be01fcfdfd
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
@ -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];
|
||||
|
|
|
@ -85,6 +85,7 @@ private:
|
|||
|
||||
SocketAPM mav_socket { false };
|
||||
bool mavlink_connected;
|
||||
mavlink_status_t mav_status;
|
||||
|
||||
void send_report(void);
|
||||
};
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue