Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

SITL: stop using mavlink channels for packing messages

Browse Source 
precedent was set in the mavlink rangefinder; we don't need to re-use SITL buffers for this
This commit is contained in:
Peter Barker 2023-05-12 18:04:12 +10:00 committed by Andrew Tridgell
parent 8cfd9f8ef1
commit 29d6c51f43
6 changed files with 117 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/GCS_MAVLink/GCS_MAVLink.h
View File

@ -14,14 +14,8 @@
#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan, size)
#define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// allow extra mavlink channels in SITL for:
// Vicon, ship
#define MAVLINK_COMM_NUM_BUFFERS 7
#else
// allow five telemetry ports
#define MAVLINK_COMM_NUM_BUFFERS 5
#endif
/*
The MAVLink protocol code generator does its own alignment, so

9
libraries/SITL/SIM_RF_MAVLink.cpp
View File

@ -27,15 +27,6 @@ using namespace SITL;
uint32_t RF_MAVLink::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen)
{
// we share channels with the ArduPilot binary!
// we're swiping the Vicon's channel here. If it causes issues we
// may need to allocate an additional mavlink channel for the rangefinder
const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
if (!valid_channel(mavlink_ch)) {
AP_HAL::panic("Invalid mavlink channel");
}
mavlink_message_t msg;
const uint8_t system_id = 32;
const uint8_t component_id = 32;

105
libraries/SITL/SIM_Ship.cpp
View File

@ -28,10 +28,6 @@
#include <AP_HAL_SITL/SITL_State.h>
#include <AP_Terrain/AP_Terrain.h>
// use a spare channel for send. This is static to avoid mavlink
// header import in SIM_Ship.h
static const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+6);
using namespace SITL;
// SITL Ship parameters
@ -74,9 +70,6 @@ void Ship::update(float delta_t)
ShipSim::ShipSim()
{
if (!valid_channel(mavlink_ch)) {
AP_HAL::panic("Invalid mavlink channel for ShipSim");
}
AP_Param::setup_object_defaults(this, var_info);
}
@ -171,24 +164,25 @@ void ShipSim::send_report(void)
}
uint32_t now = AP_HAL::millis();
mavlink_message_t msg;
uint16_t len;
uint8_t buf[300];
const uint8_t component_id = MAV_COMP_ID_USER10;
if (now - last_heartbeat_ms >= 1000) {
last_heartbeat_ms = now;
mavlink_msg_heartbeat_pack_chan(sys_id.get(),
component_id,
mavlink_ch,
&msg,
MAV_TYPE_SURFACE_BOAT,
MAV_AUTOPILOT_INVALID,
0,
0,
0);
len = mavlink_msg_to_send_buffer(buf, &msg);
const mavlink_heartbeat_t heartbeat{
MAV_TYPE_SURFACE_BOAT,
MAV_AUTOPILOT_INVALID,
0,
0,
0};
mavlink_message_t msg;
mavlink_msg_heartbeat_encode(
sys_id.get(),
component_id,
&msg,
&heartbeat);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
mav_socket.send(buf, len);
}
@ -209,38 +203,51 @@ void ShipSim::send_report(void)
}
#endif
Vector2f vel(ship.speed, 0);
vel.rotate(radians(ship.heading_deg));
{ // send position
Vector2f vel(ship.speed, 0);
vel.rotate(radians(ship.heading_deg));
mavlink_msg_global_position_int_pack_chan(sys_id,
component_id,
mavlink_ch,
&msg,
now,
loc.lat,
loc.lng,
alt_mm,
0,
vel.x*100,
vel.y*100,
0,
ship.heading_deg*100);
len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
const mavlink_global_position_int_t global_position_int{
now,
loc.lat,
loc.lng,
alt_mm,
0,
int16_t(vel.x*100),
int16_t(vel.y*100),
0,
uint16_t(ship.heading_deg*100)
};
mavlink_message_t msg;
mavlink_msg_global_position_int_encode(
sys_id,
component_id,
&msg,
&global_position_int);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
}
}
// also set ATTITUDE so MissionPlanner can display ship orientation
mavlink_msg_attitude_pack_chan(sys_id,
component_id,
mavlink_ch,
&msg,
now,
0, 0, radians(ship.heading_deg),
0, 0, ship.yaw_rate);
len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
{ // also set ATTITUDE so MissionPlanner can display ship orientation
const mavlink_attitude_t attitude{
now,
0, 0, float(radians(ship.heading_deg)),
0, 0, ship.yaw_rate
};
mavlink_message_t msg;
mavlink_msg_attitude_encode(
sys_id,
component_id,
&msg,
&attitude);
uint8_t buf[300];
const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
if (len > 0) {
mav_socket.send(buf, len);
}
}
}

1
libraries/SITL/SIM_Ship.h
View File

@ -29,6 +29,7 @@
#include <AP_HAL/utility/Socket.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/Location.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
namespace SITL {

115
libraries/SITL/SIM_Vicon.cpp
View File

@ -31,9 +31,6 @@ using namespace SITL;
Vicon::Vicon() :
SerialDevice::SerialDevice()
{
if (!valid_channel(mavlink_ch)) {
AP_HAL::panic("Invalid mavlink channel");
}
}
void Vicon::maybe_send_heartbeat()
@ -158,52 +155,57 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
// send vision position estimate message
uint8_t msg_buf_index;
if (should_send(ViconTypeMask::VISION_POSITION_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
mavlink_msg_vision_position_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg_buf[msg_buf_index].obs_msg,
const mavlink_vision_position_estimate_t vision_position_estimate{
now_us + time_offset_us,
pos_corrected.x,
pos_corrected.y,
pos_corrected.z,
float(pos_corrected.x),
float(pos_corrected.y),
float(pos_corrected.z),
roll,
pitch,
yaw,
NULL, 0);
yaw
};
mavlink_msg_vision_position_estimate_encode(
system_id,
component_id,
&msg_buf[msg_buf_index].obs_msg,
&vision_position_estimate
);
msg_buf[msg_buf_index].time_send_us = time_send_us;
}
// send older vicon position estimate message
if (should_send(ViconTypeMask::VICON_POSITION_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
mavlink_msg_vicon_position_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg_buf[msg_buf_index].obs_msg,
const mavlink_vicon_position_estimate_t vicon_position_estimate{
now_us + time_offset_us,
pos_corrected.x,
pos_corrected.y,
pos_corrected.z,
float(pos_corrected.x),
float(pos_corrected.y),
float(pos_corrected.z),
roll,
pitch,
yaw,
NULL);
yaw
};
mavlink_msg_vicon_position_estimate_encode(
system_id,
component_id,
&msg_buf[msg_buf_index].obs_msg,
&vicon_position_estimate);
msg_buf[msg_buf_index].time_send_us = time_send_us;
}
// send vision speed estimate
if (should_send(ViconTypeMask::VISION_SPEED_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
mavlink_msg_vision_speed_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg_buf[msg_buf_index].obs_msg,
const mavlink_vision_speed_estimate_t vicon_speed_estimate{
now_us + time_offset_us,
vel_corrected.x,
vel_corrected.y,
vel_corrected.z,
NULL, 0);
vel_corrected.z
};
mavlink_msg_vision_speed_estimate_encode(
system_id,
component_id,
&msg_buf[msg_buf_index].obs_msg,
&vicon_speed_estimate
);
msg_buf[msg_buf_index].time_send_us = time_send_us;
}
@ -211,27 +213,30 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
// send ODOMETRY message
if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) {
const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
mavlink_msg_odometry_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg_buf[msg_buf_index].obs_msg,
const mavlink_odometry_t odometry{
now_us + time_offset_us,
MAV_FRAME_LOCAL_FRD,
MAV_FRAME_BODY_FRD,
pos_corrected.x,
pos_corrected.y,
pos_corrected.z,
&attitude[0],
float(pos_corrected.x),
float(pos_corrected.y),
float(pos_corrected.z),
{attitude[0], attitude[1], attitude[2], attitude[3]},
vel_corrected_frd.x,
vel_corrected_frd.y,
vel_corrected_frd.z,
gyro.x,
gyro.y,
gyro.z,
NULL, NULL,
{},
{},
MAV_FRAME_LOCAL_FRD,
MAV_FRAME_BODY_FRD,
0,
MAV_ESTIMATOR_TYPE_VIO);
MAV_ESTIMATOR_TYPE_VIO
};
mavlink_msg_odometry_encode(
system_id,
component_id,
&msg_buf[msg_buf_index].obs_msg,
&odometry);
msg_buf[msg_buf_index].time_send_us = time_send_us;
}
@ -243,15 +248,11 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
attitude_curr.invert();
Quaternion attitude_curr_prev = attitude_curr * _attitude_prev.inverse(); // Get rotation to current MAV_FRAME_BODY_FRD from previous MAV_FRAME_BODY_FRD
float angle_delta[3] = {attitude_curr_prev.get_euler_roll(),
attitude_curr_prev.get_euler_pitch(),
attitude_curr_prev.get_euler_yaw()};
Matrix3f body_ned_m;
attitude_curr.rotation_matrix(body_ned_m);
Vector3f pos_delta = body_ned_m * (pos_corrected - _position_prev).tofloat();
float postion_delta[3] = {pos_delta.x, pos_delta.y, pos_delta.z};
// send vision position delta
// time_usec: (usec) Current time stamp
@ -260,16 +261,20 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
// delta_position [3]: (meters) Change in position: To current position from previous position rotated to current MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED
// confidence: Normalized confidence level [0, 100]
if (should_send(ViconTypeMask::VISION_POSITION_DELTA) && get_free_msg_buf_index(msg_buf_index)) {
mavlink_msg_vision_position_delta_pack_chan(
system_id,
component_id,
mavlink_ch,
&msg_buf[msg_buf_index].obs_msg,
const mavlink_vision_position_delta_t vision_position_delta{
now_us + time_offset_us,
time_delta,
angle_delta,
postion_delta,
0.0f);
{ attitude_curr_prev.get_euler_roll(),
attitude_curr_prev.get_euler_pitch(),
attitude_curr_prev.get_euler_yaw()
},
{pos_delta.x, pos_delta.y, pos_delta.z}
};
mavlink_msg_vision_position_delta_encode(
system_id,
component_id,
&msg_buf[msg_buf_index].obs_msg,
&vision_position_delta);
msg_buf[msg_buf_index].time_send_us = time_send_us;
}

4
libraries/SITL/SIM_Vicon.h
View File

@ -42,10 +42,6 @@ private:
const uint8_t system_id = 17;
const uint8_t component_id = 18;
// we share channels with the ArduPilot binary!
// Beware: the mavlink rangefinder shares this channel.
const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
uint64_t last_observation_usec; // time last observation was sent
uint64_t time_offset_us; // simulated timeoffset between external system and autopilot