Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

fix(mavlink): MISSION_ITEM deprecated

Browse Source
This commit is contained in:
Hamish Willee
2026-05-15 09:21:39 +10:00
parent daa89a9116
commit 1dc379a7f1
2 changed files with 54 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_mission.cpp
+47 -160
View File
@@ -364,32 +364,17 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
if (read_success) {
_time_last_sent = hrt_absolute_time();
if (_int_mode) {
mavlink_mission_item_int_t wp{};
format_mavlink_mission_item(&mission_item, reinterpret_cast<mavlink_mission_item_t *>(&wp));
mavlink_mission_item_int_t wp{};
format_mavlink_mission_item(&mission_item, reinterpret_cast<mavlink_mission_item_t *>(&wp));
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_int_send_struct(_mavlink.get_channel(), &wp);
mavlink_msg_mission_item_int_send_struct(_mavlink.get_channel(), &wp);
PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
} else {
mavlink_mission_item_t wp{};
format_mavlink_mission_item(&mission_item, &wp);
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_send_struct(_mavlink.get_channel(), &wp);
PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system);
}
PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
} else {
send_mission_ack(sysid, compid, MAV_MISSION_ERROR);
@@ -447,28 +432,14 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
if (seq < current_max_item_count()) {
_time_last_sent = hrt_absolute_time();
if (_int_mode) {
mavlink_mission_request_int_t wpr{};
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
wpr.mission_type = _mission_type;
mavlink_msg_mission_request_int_send_struct(_mavlink.get_channel(), &wpr);
mavlink_mission_request_int_t wpr{};
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
wpr.mission_type = _mission_type;
mavlink_msg_mission_request_int_send_struct(_mavlink.get_channel(), &wpr);
PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
} else {
mavlink_mission_request_t wpr{};
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
wpr.mission_type = _mission_type;
mavlink_msg_mission_request_send_struct(_mavlink.get_channel(), &wpr);
PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system);
}
PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
} else {
_mavlink.send_statustext_critical("ERROR: Waypoint index exceeds list capacity\t");
@@ -611,11 +582,8 @@ MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
handle_mission_request(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
handle_mission_request_int(msg);
handle_mission_request_both(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
@@ -665,19 +633,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
// INT or float mode is not supported
if (wpa.type == MAV_MISSION_UNSUPPORTED) {
if (_int_mode) {
_int_mode = false;
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
} else {
_int_mode = true;
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
}
} else if (wpa.type == MAV_MISSION_OPERATION_CANCELLED) {
if (wpa.type == MAV_MISSION_OPERATION_CANCELLED) {
PX4_DEBUG("WPM: MISSION_ACK CANCELLED, switch to state IDLE");
switch_to_idle_state();
_transfer_in_progress = false;
@@ -791,28 +747,6 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
}
void
MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
{
// The request comes in the old float mode, so we switch to it.
if (_int_mode) {
_int_mode = false;
}
handle_mission_request_both(msg);
}
void
MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg)
{
// The request comes in the new int mode, so we switch to it.
if (!_int_mode) {
_int_mode = true;
}
handle_mission_request_both(msg);
}
void
MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
{
@@ -1055,32 +989,21 @@ MavlinkMissionManager::switch_to_idle_state()
void
MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
{
if (_int_mode) {
// It seems that we should be using the float mode, let's switch out of int mode.
_int_mode = false;
}
handle_mission_item_both(msg);
_mavlink.send_statustext_info("WPM: received deprecated MISSION_ITEM, use MISSION_ITEM_INT\t");
handle_mission_item_both(msg, false);
}
void
MavlinkMissionManager::handle_mission_item_int(const mavlink_message_t *msg)
{
if (!_int_mode) {
// It seems that we should be using the int mode, let's switch to it.
_int_mode = true;
}
handle_mission_item_both(msg);
handle_mission_item_both(msg, true);
}
void
MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg, bool int_mode)
{
// The mavlink_message could also contain a mavlink_mission_item_int_t. We ignore that here
// and take care of it later in parse_mavlink_mission_item depending on _int_mode.
// The mavlink_message may also contain a mavlink_mission_item_int_t; both structs have
// the same layout, so we decode as mavlink_mission_item_t and pass int_mode to the parser.
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
@@ -1131,7 +1054,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
struct mission_item_s mission_item = {};
int ret = parse_mavlink_mission_item(&wp, &mission_item);
int ret = parse_mavlink_mission_item(&wp, int_mode, &mission_item);
if (ret != PX4_OK) {
PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
@@ -1403,22 +1326,19 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
struct mission_item_s *mission_item)
bool int_mode, struct mission_item_s *mission_item)
{
if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
(_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) {
// This is a mission item with a global coordinate
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
// This is a mission item with a global coordinate.
// Detect int vs float from the frame field — no need for the int_mode parameter here.
const bool frame_is_int = (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT);
// Switch to int mode if that is what we are receiving
if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) {
_int_mode = true;
}
if (_int_mode) {
/* The argument is actually a mavlink_mission_item_int_t in int_mode.
if (frame_is_int) {
/* The argument is actually a mavlink_mission_item_int_t.
* mavlink_mission_item_t and mavlink_mission_item_int_t have the same
* alignment, so we can just swap float for int32_t. */
const mavlink_mission_item_int_t *item_int
@@ -1571,8 +1491,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->params[2] = mavlink_mission_item->param3;
mission_item->params[3] = mavlink_mission_item->param4;
if (_int_mode) {
/* The argument is actually a mavlink_mission_item_int_t in int_mode.
if (int_mode) {
/* The argument is actually a mavlink_mission_item_int_t.
* mavlink_mission_item_t and mavlink_mission_item_int_t have the same
* alignment, so we can just swap float for int32_t. */
const mavlink_mission_item_int_t *item_int
@@ -1687,23 +1607,13 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
mavlink_mission_item->param3 = mission_item->params[2];
mavlink_mission_item->param4 = mission_item->params[3];
mavlink_mission_item->x = mission_item->params[4];
mavlink_mission_item->y = mission_item->params[5];
// This function receives a mavlink_mission_item_int_t cast as mavlink_mission_item_t;
// both structs have the same alignment — only int32_t vs. float differs for x and y.
mavlink_mission_item_int_t *item_int =
reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
if (_int_mode) {
// This function actually receives a mavlink_mission_item_int_t in _int_mode
// which has the same alignment as mavlink_mission_item_t and the only
// difference is int32_t vs. float for x and y.
mavlink_mission_item_int_t *item_int =
reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
item_int->x = round(mission_item->params[4]);
item_int->y = round(mission_item->params[5]);
} else {
mavlink_mission_item->x = (float)mission_item->params[4];
mavlink_mission_item->y = (float)mission_item->params[5];
}
item_int->x = round(mission_item->params[4]);
item_int->y = round(mission_item->params[5]);
mavlink_mission_item->z = mission_item->params[6];
@@ -1748,39 +1658,16 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
mavlink_mission_item->param3 = 0.0f;
mavlink_mission_item->param4 = 0.0f;
if (_int_mode) {
// This function actually receives a mavlink_mission_item_int_t in _int_mode
// which has the same alignment as mavlink_mission_item_t and the only
// difference is int32_t vs. float for x and y.
mavlink_mission_item_int_t *item_int =
reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
mavlink_mission_item_int_t *item_int =
reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
item_int->x = round(mission_item->lat * 1e7);
item_int->y = round(mission_item->lon * 1e7);
} else {
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
}
item_int->x = round(mission_item->lat * 1e7);
item_int->y = round(mission_item->lon * 1e7);
mavlink_mission_item->z = mission_item->altitude;
if (mission_item->altitude_is_relative) {
if (_int_mode) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
} else {
if (_int_mode) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
}
}
mavlink_mission_item->frame = mission_item->altitude_is_relative ?
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_INT;
switch (mission_item->nav_cmd) {
case NAV_CMD_WAYPOINT:
src/modules/mavlink/mavlink_mission.h
+7 -11
View File
@@ -114,8 +114,6 @@ private:
uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint
bool _int_mode{true}; ///< Use accurate int32 instead of float
unsigned _filesystem_errcount{0}; ///< File system error count
static dm_item_t _mission_dataman_id; ///< Global Dataman storage ID for active mission
@@ -238,33 +236,31 @@ private:
void handle_mission_request_list(const mavlink_message_t *msg);
void handle_mission_request(const mavlink_message_t *msg);
void handle_mission_request_int(const mavlink_message_t *msg);
void handle_mission_request_both(const mavlink_message_t *msg);
void handle_mission_count(const mavlink_message_t *msg);
void handle_mission_item(const mavlink_message_t *msg);
void handle_mission_item_int(const mavlink_message_t *msg);
void handle_mission_item_both(const mavlink_message_t *msg);
void handle_mission_item_both(const mavlink_message_t *msg, bool int_mode);
void handle_mission_clear_all(const mavlink_message_t *msg);
/**
* Parse mavlink MISSION_ITEM message to get mission_item_s.
* Parse mavlink MISSION_ITEM(_INT) message to get mission_item_s.
*
* @param mavlink_mission_item pointer to mavlink_mission_item_t or mavlink_mission_item_int_t
* depending on _int_mode
* @param int_mode true if the message is a mavlink_mission_item_int_t
* @param mission_item pointer to mission_item to construct
*/
int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, bool int_mode,
struct mission_item_s *mission_item);
/**
* Format mission_item_s as mavlink MISSION_ITEM(_INT) message.
* Format mission_item_s as mavlink MISSION_ITEM_INT message.
*
* @param mission_item: pointer to the existing mission item
* @param mavlink_mission_item: pointer to mavlink_mission_item_t or mavlink_mission_item_int_t
* depending on _int_mode.
* @param mavlink_mission_item: pointer to mavlink_mission_item_int_t cast as mavlink_mission_item_t
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item,
mavlink_mission_item_t *mavlink_mission_item);