fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink-mission: Add support for opaque ids and replace update counter with it

Browse Source
This commit is contained in:
Konrad
2023-11-29 11:10:40 -05:00
committed by Daniel Agar
parent 120e7fea8b
commit 36f0c0f0bf
17 changed files with 194 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_mission.cpp
+93 -52
View File
@@ -53,19 +53,17 @@
#include <navigator/navigation.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <crc32.h>
using matrix::wrap_2pi;
dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
bool MavlinkMissionManager::_dataman_init = false;
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 };
int32_t MavlinkMissionManager::_current_seq = 0;
bool MavlinkMissionManager::_transfer_in_progress = false;
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
uint16_t MavlinkMissionManager::_mission_update_counter = 0;
uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
@@ -96,14 +94,14 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
}
void
MavlinkMissionManager::init_offboard_mission(mission_s mission_state)
MavlinkMissionManager::init_offboard_mission(const mission_s &mission_state)
{
_dataman_id = (dm_item_t)mission_state.dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
_crc32[MAV_MISSION_TYPE_MISSION] = mission_state.mission_id;
_current_seq = mission_state.current_seq;
_land_start_marker = mission_state.land_start_index;
_land_marker = mission_state.land_index;
_mission_update_counter = mission_state.mission_update_counter;
}
bool
@@ -116,7 +114,7 @@ MavlinkMissionManager::load_geofence_stats()
if (success) {
_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
_geofence_update_counter = stats.update_counter;
_crc32[MAV_MISSION_TYPE_FENCE] = stats.opaque_id;
}
return success;
@@ -132,7 +130,7 @@ MavlinkMissionManager::load_safepoint_stats()
if (success) {
_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
_safepoint_update_counter = stats.update_counter;
_crc32[MAV_MISSION_TYPE_RALLY] = stats.opaque_id;
}
return success;
@@ -142,25 +140,27 @@ MavlinkMissionManager::load_safepoint_stats()
* Publish mission topic to notify navigator about changes.
*/
void
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman)
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32,
bool write_to_dataman)
{
/* update active mission state */
_dataman_id = dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = count;
_crc32[MAV_MISSION_TYPE_MISSION] = crc32;
_current_seq = seq;
_my_dataman_id = _dataman_id;
mission_s mission{};
mission.timestamp = hrt_absolute_time();
mission.dataman_id = dataman_id;
mission.count = count;
mission.current_seq = seq;
mission.mission_update_counter = _mission_update_counter;
mission.geofence_update_counter = _geofence_update_counter;
mission.safe_points_update_counter = _safepoint_update_counter;
mission.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
mission.geofence_id = _crc32[MAV_MISSION_TYPE_FENCE];
mission.safe_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
mission.land_start_index = _land_start_marker;
mission.land_index = _land_marker;
/* update active mission state */
_dataman_id = dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = count;
_current_seq = seq;
_my_dataman_id = _dataman_id;
if (write_to_dataman) {
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),
sizeof(mission_s));
@@ -174,11 +174,11 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
}
int
MavlinkMissionManager::update_geofence_count(unsigned count)
MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
{
mission_stats_entry_s stats;
stats.num_items = count;
stats.update_counter = ++_geofence_update_counter;
stats.opaque_id = crc32;
/* update stats in dataman */
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
@@ -186,6 +186,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
if (success) {
_count[MAV_MISSION_TYPE_FENCE] = count;
_crc32[MAV_MISSION_TYPE_FENCE] = crc32;
} else {
@@ -198,16 +199,17 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
return PX4_ERROR;
}
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false);
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION],
false);
return PX4_OK;
}
int
MavlinkMissionManager::update_safepoint_count(unsigned count)
MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
{
mission_stats_entry_s stats;
stats.num_items = count;
stats.update_counter = ++_safepoint_update_counter;
stats.opaque_id = crc32;
/* update stats in dataman */
bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
@@ -215,6 +217,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
if (success) {
_count[MAV_MISSION_TYPE_RALLY] = count;
_crc32[MAV_MISSION_TYPE_RALLY] = crc32;
} else {
@@ -227,12 +230,13 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
return PX4_ERROR;
}
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false);
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, _crc32[MAV_MISSION_TYPE_MISSION],
false);
return PX4_OK;
}
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type, uint32_t opaque_id)
{
mavlink_mission_ack_t wpa{};
@@ -240,6 +244,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
wpa.target_component = compid;
wpa.type = type;
wpa.mission_type = _mission_type;
wpa.opaque_id = opaque_id;
mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa);
@@ -251,13 +256,17 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
{
mavlink_mission_current_t wpc{};
wpc.seq = seq;
wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE];
wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq);
}
void
MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type,
uint32_t opaque_id)
{
_time_last_sent = hrt_absolute_time();
@@ -267,6 +276,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
wpc.target_component = compid;
wpc.count = count;
wpc.mission_type = mission_type;
wpc.opaque_id = opaque_id;
mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc);
@@ -390,6 +400,17 @@ MavlinkMissionManager::current_item_count()
return _count[_mission_type];
}
uint32_t
MavlinkMissionManager::get_current_mission_type_crc()
{
if (_mission_type >= sizeof(_crc32) / sizeof(_crc32[0])) {
PX4_ERR("WPM: _crc32 out of bounds (%u)", _mission_type);
return 0;
}
return _crc32[_mission_type];
}
void
MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
@@ -649,7 +670,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
_time_last_recv = hrt_absolute_time();
if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) {
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq);
update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq, _crc32[MAV_MISSION_TYPE_MISSION]);
} else {
PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq);
@@ -718,7 +739,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type);
}
send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type);
send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type, get_current_mission_type_crc());
} else {
PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy");
@@ -862,6 +883,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
_transfer_in_progress = true;
_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
_transfer_current_crc32 = 0;
if (wpc.count > current_max_item_count()) {
PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
@@ -879,25 +901,24 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
_land_start_marker = -1;
_land_marker = -1;
++_mission_update_counter;
/* alternate dataman ID anyway to let navigator know about changes */
if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0);
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0, 0);
} else {
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
}
break;
case MAV_MISSION_TYPE_FENCE:
update_geofence_count(0);
update_geofence_count(0, 0);
break;
case MAV_MISSION_TYPE_RALLY:
update_safepoint_count(0);
update_safepoint_count(0, 0);
break;
default:
@@ -1016,7 +1037,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
if (_transfer_seq == wp.seq + 1) {
// Assume this is a duplicate, where we already successfully got all mission items,
// but the GCS did not receive the last ack and sent the same item again
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
} else {
PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer");
@@ -1056,6 +1077,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
return;
}
_transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32);
bool write_failed = false;
bool check_failed = false;
@@ -1115,7 +1138,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
if (mission_item.vertex_count < 3) { // feasibility check
PX4_ERR("Fence: too few vertices");
check_failed = true;
update_geofence_count(0);
update_geofence_count(0, 0);
}
} else {
@@ -1179,18 +1202,17 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION:
++_mission_update_counter;
_land_start_marker = _transfer_land_start_marker;
_land_marker = _transfer_land_marker;
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
break;
case MAV_MISSION_TYPE_FENCE:
ret = update_geofence_count(_transfer_count);
ret = update_geofence_count(_transfer_count, _transfer_current_crc32);
break;
case MAV_MISSION_TYPE_RALLY:
ret = update_safepoint_count(_transfer_count);
ret = update_safepoint_count(_transfer_count, _transfer_current_crc32);
break;
default:
@@ -1203,7 +1225,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
if (ret == PX4_OK) {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32);
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
@@ -1236,29 +1258,27 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
switch (wpca.mission_type) {
case MAV_MISSION_TYPE_MISSION:
++_mission_update_counter;
_land_start_marker = -1;
_land_marker = -1;
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
break;
case MAV_MISSION_TYPE_FENCE:
ret = update_geofence_count(0);
ret = update_geofence_count(0, 0);
break;
case MAV_MISSION_TYPE_RALLY:
ret = update_safepoint_count(0);
ret = update_safepoint_count(0, 0);
break;
case MAV_MISSION_TYPE_ALL:
++_mission_update_counter;
_land_start_marker = -1;
_land_marker = -1;
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
ret = update_geofence_count(0);
ret = update_safepoint_count(0) || ret;
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
ret = update_geofence_count(0, 0);
ret = update_safepoint_count(0, 0) || ret;
break;
default:
@@ -1761,21 +1781,42 @@ void MavlinkMissionManager::check_active_mission()
if (_mission_sub.updated()) {
_mission_sub.update();
if (_mission_sub.get().geofence_update_counter != _geofence_update_counter) {
if (_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) {
load_geofence_stats();
}
if (_mission_sub.get().safe_points_update_counter != _safepoint_update_counter) {
if (_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) {
load_safepoint_stats();
}
if ((_mission_sub.get().mission_update_counter != _mission_update_counter)
if ((_mission_sub.get().mission_id != _crc32[MAV_MISSION_TYPE_MISSION])
|| (_my_dataman_id != (dm_item_t)_mission_sub.get().dataman_id)) {
PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
init_offboard_mission(_mission_sub.get());
_my_dataman_id = _dataman_id;
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION],
MAV_MISSION_TYPE_MISSION);
MAV_MISSION_TYPE_MISSION, _crc32[MAV_MISSION_TYPE_MISSION]);
}
}
}
uint32_t MavlinkMissionManager::crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32)
{
union {
CrcMissionItem_t item;
uint8_t raw[sizeof(CrcMissionItem_t)];
} u;
u.item.frame = mission_item.frame;
u.item.command = mission_item.command;
u.item.autocontinue = mission_item.autocontinue;
u.item.params[0] = mission_item.param1;
u.item.params[1] = mission_item.param2;
u.item.params[2] = mission_item.param3;
u.item.params[3] = mission_item.param4;
u.item.params[4] = mission_item.x;
u.item.params[5] = mission_item.y;
u.item.params[6] = mission_item.z;
return crc32part(u.raw, sizeof(u), prev_crc32);
}
src/modules/mavlink/mavlink_mission.h
+21 -9
View File
@@ -112,6 +112,7 @@ private:
static bool _dataman_init; ///< Dataman initialized
static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
static uint32_t _crc32[3]; ///< Checksum of items in (active) mission for each MAV_MISSION_TYPE
static int32_t _current_seq; ///< Current item sequence in active mission
int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached)
@@ -119,6 +120,7 @@ private:
dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission
uint16_t _transfer_count{0}; ///< Items count in current transmission
uint32_t _transfer_current_crc32{0}; ///< Current CRC32 checksum of current transmission
uint16_t _transfer_seq{0}; ///< Item sequence in current transmission
int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized)
@@ -135,9 +137,6 @@ private:
uORB::Publication<mission_s> _offboard_mission_pub{ORB_ID(mission)};
static uint16_t _mission_update_counter;
static uint16_t _geofence_update_counter;
static uint16_t _safepoint_update_counter;
int32_t _land_start_marker{-1}; ///< index of loaded land start mission item (if unavailable, index of land mission item, -1 otherwise)
int32_t _land_marker{-1}; ///< index of loaded land mission item (-1 if unavailable)
@@ -160,19 +159,23 @@ private:
/** get the number of item count for the current _mission_type */
uint16_t current_item_count();
/** get the crc32 checksum for the current _mission_type */
uint32_t get_current_mission_type_crc();
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager &operator = (const MavlinkMissionManager &);
void init_offboard_mission(mission_s mission_state);
void init_offboard_mission(const mission_s &mission_state);
void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman = true);
void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32,
bool write_to_dataman = true);
/** store the geofence count to dataman */
int update_geofence_count(unsigned count);
int update_geofence_count(unsigned count, uint32_t crc32);
/** store the safepoint count to dataman */
int update_safepoint_count(unsigned count);
int update_safepoint_count(unsigned count, uint32_t crc32);
/** load geofence stats from dataman */
bool load_geofence_stats();
@@ -183,7 +186,7 @@ private:
/**
* @brief Sends an waypoint ack message
*/
void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type, uint32_t opaque_id = 0U);
/**
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
@@ -196,7 +199,8 @@ private:
*/
void send_mission_current(uint16_t seq);
void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type);
void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type,
uint32_t opaque_id);
void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
@@ -267,4 +271,12 @@ private:
*/
void copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx = 1, int8_t end_idx = 7);
/**
* Update crc calculation including new mission item
* @param[in] mission_item new mission item
* @param[in] prev_crc32 crc32 checksum of all previous mission item
* @return updated crc32 checksum of mission items
*/
static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32);
};