forked from Archive/PX4-Autopilot
mavlink-mission: Add support for opaque ids and replace update counter with it
This commit is contained in:
Konrad
Daniel Agar
parent
120e7fea8b
commit
36f0c0f0bf
|
|
@ -7,6 +7,6 @@ int32 current_seq # default -1, start at the one changed latest
|
|||
int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise
|
||||
int32 land_index # Index of the land item, -1 otherwise
|
||||
|
||||
uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
|
||||
uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
|
||||
uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased
|
||||
uint32 mission_id # indicates updates to the mission, reload from dataman if changed
|
||||
uint32 geofence_id # indicates updates to the geofence, reload from dataman if changed
|
||||
uint32 safe_points_id # indicates updates to the safe points, reload from dataman if changed
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint16 mission_update_counter # Counter for the mission for which the result was generated
|
||||
uint16 geofence_update_counter # Counter for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint16 mission_id # Id for the mission for which the result was generated
|
||||
uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility)
|
||||
|
||||
int32 seq_reached # Sequence of the mission item which has been reached, default -1
|
||||
|
|
|
|||
|
|
@ -1935,19 +1935,19 @@ void Commander::checkForMissionUpdate()
|
|||
if (_mission_result_sub.updated()) {
|
||||
const mission_result_s &mission_result = _mission_result_sub.get();
|
||||
|
||||
const auto prev_mission_mission_update_counter = mission_result.mission_update_counter;
|
||||
const auto prev_mission_mission_id = mission_result.mission_id;
|
||||
_mission_result_sub.update();
|
||||
|
||||
// if mission_result is valid for the current mission
|
||||
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
|
||||
&& (mission_result.mission_update_counter > 0);
|
||||
&& (mission_result.mission_id > 0);
|
||||
|
||||
bool auto_mission_available = mission_result_ok && mission_result.valid;
|
||||
|
||||
if (mission_result_ok) {
|
||||
/* Only evaluate mission state if home is set */
|
||||
if (!_failsafe_flags.home_position_invalid &&
|
||||
(prev_mission_mission_update_counter != mission_result.mission_update_counter)) {
|
||||
(prev_mission_mission_id != mission_result.mission_id)) {
|
||||
|
||||
if (!auto_mission_available) {
|
||||
/* the mission is invalid */
|
||||
|
|
|
|||
|
|
@ -56,6 +56,6 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
}
|
||||
|
||||
// This is a mode requirement, no need to report
|
||||
reporter.failsafeFlags().auto_mission_missing = mission_result.mission_update_counter <= 0;
|
||||
reporter.failsafeFlags().auto_mission_missing = mission_result.mission_id <= 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -574,10 +574,13 @@ _file_initialize(unsigned max_offset)
|
|||
mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
mission.count = 0;
|
||||
mission.current_seq = 0;
|
||||
mission.mission_id = 0u;
|
||||
mission.geofence_id = 0u;
|
||||
mission.safe_points_id = 0u;
|
||||
|
||||
mission_stats_entry_s stats;
|
||||
stats.num_items = 0;
|
||||
stats.update_counter = 1;
|
||||
stats.opaque_id = 0;
|
||||
|
||||
g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission), sizeof(mission_s));
|
||||
g_dm_ops->write(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats), sizeof(mission_stats_entry_s));
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -40,8 +40,10 @@
|
|||
*/
|
||||
#include "geofence.h"
|
||||
#include "navigator.h"
|
||||
#include "navigation.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <crc32.h>
|
||||
|
||||
#include <dataman_client/DatamanClient.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
|
@ -51,6 +53,27 @@
|
|||
|
||||
#include "navigator.h"
|
||||
|
||||
static uint32_t crc32_for_fence_point(const mission_fence_point_s &fence_point, uint32_t prev_crc32)
|
||||
{
|
||||
union {
|
||||
CrcMissionItem_t item;
|
||||
uint8_t raw[sizeof(CrcMissionItem_t)];
|
||||
} u;
|
||||
|
||||
u.item.frame = fence_point.frame;
|
||||
u.item.command = fence_point.nav_cmd;
|
||||
u.item.autocontinue = 0U;
|
||||
u.item.params[0] = 0.f;
|
||||
u.item.params[1] = 0.f;
|
||||
u.item.params[2] = 0.f;
|
||||
u.item.params[3] = 0.f;
|
||||
u.item.params[4] = static_cast<float>(fence_point.lat);
|
||||
u.item.params[5] = static_cast<float>(fence_point.lon);
|
||||
u.item.params[6] = fence_point.alt;
|
||||
|
||||
return crc32part(u.raw, sizeof(u), prev_crc32);
|
||||
}
|
||||
|
||||
Geofence::Geofence(Navigator *navigator) :
|
||||
ModuleParams(navigator),
|
||||
_navigator(navigator)
|
||||
|
|
@ -105,9 +128,9 @@ void Geofence::run()
|
|||
_error_state = DatamanState::ReadWait;
|
||||
_dataman_state = DatamanState::Error;
|
||||
|
||||
} else if (_update_counter != _stats.update_counter) {
|
||||
} else if (_opaque_id != _stats.opaque_id) {
|
||||
|
||||
_update_counter = _stats.update_counter;
|
||||
_opaque_id = _stats.opaque_id;
|
||||
_fence_updated = false;
|
||||
|
||||
_dataman_cache.invalidate();
|
||||
|
|
@ -559,6 +582,7 @@ Geofence::loadFromFile(const char *filename)
|
|||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
|
||||
events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
|
||||
ret_val = PX4_ERROR;
|
||||
uint32_t crc32{0U};
|
||||
|
||||
/* do a second pass, now that we know the number of vertices */
|
||||
for (int seq = 1; seq <= pointCounter; ++seq) {
|
||||
|
|
@ -569,6 +593,7 @@ Geofence::loadFromFile(const char *filename)
|
|||
|
||||
if (success) {
|
||||
mission_fence_point.vertex_count = pointCounter;
|
||||
crc32 = crc32_for_fence_point(mission_fence_point, crc32);
|
||||
_dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
sizeof(mission_fence_point_s));
|
||||
}
|
||||
|
|
@ -576,7 +601,7 @@ Geofence::loadFromFile(const char *filename)
|
|||
|
||||
mission_stats_entry_s stats;
|
||||
stats.num_items = pointCounter;
|
||||
stats.update_counter = _update_counter + 1;
|
||||
stats.opaque_id = crc32;
|
||||
|
||||
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
|
||||
sizeof(mission_stats_entry_s));
|
||||
|
|
|
|||
|
|
@ -183,8 +183,7 @@ private:
|
|||
|
||||
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
|
||||
|
||||
|
||||
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, polygon data was updated
|
||||
uint32_t _opaque_id{0}; ///< dataman geofence id: if it does not match, the polygon data was updated
|
||||
bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache
|
||||
bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed
|
||||
|
||||
|
|
|
|||
|
|
@ -473,7 +473,7 @@ Mission::save_mission_state()
|
|||
if (success) {
|
||||
/* data read successfully, check dataman ID and items count */
|
||||
if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count
|
||||
&& mission_state.mission_update_counter == _mission.mission_update_counter) {
|
||||
&& mission_state.mission_id == _mission.mission_id) {
|
||||
/* navigator may modify only sequence, write modified state only if it changed */
|
||||
if (mission_state.current_seq != _mission.current_seq) {
|
||||
mission_state = _mission;
|
||||
|
|
|
|||
|
|
@ -114,7 +114,7 @@ void MissionBase::updateMavlinkMission()
|
|||
if (isMissionValid(new_mission)) {
|
||||
/* Relevant mission items updated externally*/
|
||||
if (checkMissionDataChanged(new_mission)) {
|
||||
bool mission_items_changed = (new_mission.mission_update_counter != _mission.mission_update_counter);
|
||||
bool mission_items_changed = (new_mission.mission_id != _mission.mission_id);
|
||||
|
||||
if (new_mission.current_seq < 0) {
|
||||
new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast<int32_t>(new_mission.count) - 1),
|
||||
|
|
@ -689,12 +689,12 @@ MissionBase::checkMissionRestart()
|
|||
void
|
||||
MissionBase::check_mission_valid()
|
||||
{
|
||||
if ((_navigator->get_mission_result()->mission_update_counter != _mission.mission_update_counter)
|
||||
|| (_navigator->get_mission_result()->geofence_update_counter != _mission.geofence_update_counter)
|
||||
if ((_navigator->get_mission_result()->mission_id != _mission.mission_id)
|
||||
|| (_navigator->get_mission_result()->geofence_id != _mission.geofence_id)
|
||||
|| (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) {
|
||||
|
||||
_navigator->get_mission_result()->mission_update_counter = _mission.mission_update_counter;
|
||||
_navigator->get_mission_result()->geofence_update_counter = _mission.geofence_update_counter;
|
||||
_navigator->get_mission_result()->mission_id = _mission.mission_id;
|
||||
_navigator->get_mission_result()->geofence_id = _mission.geofence_id;
|
||||
_navigator->get_mission_result()->home_position_counter = _navigator->get_home_position()->update_count;
|
||||
|
||||
MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client);
|
||||
|
|
@ -1153,7 +1153,7 @@ void MissionBase::resetMission()
|
|||
new_mission.land_start_index = -1;
|
||||
new_mission.land_index = -1;
|
||||
new_mission.count = 0u;
|
||||
new_mission.mission_update_counter = _mission.mission_update_counter + 1;
|
||||
new_mission.mission_id = 0u;
|
||||
new_mission.dataman_id = _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
|
|
@ -1355,8 +1355,8 @@ void MissionBase::checkClimbRequired(int32_t mission_item_index)
|
|||
|
||||
bool MissionBase::checkMissionDataChanged(mission_s new_mission)
|
||||
{
|
||||
/* count and land_index are the same if the mission_counter did not change. We do not care about changes in geofence or rally counters.*/
|
||||
/* count and land_index are the same if the mission_id did not change. We do not care about changes in geofence or rally counters.*/
|
||||
return ((new_mission.dataman_id != _mission.dataman_id) ||
|
||||
(new_mission.mission_update_counter != _mission.mission_update_counter) ||
|
||||
(new_mission.mission_id != _mission.mission_id) ||
|
||||
(new_mission.current_seq != _mission.current_seq));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -203,8 +203,9 @@ struct mission_item_s {
|
|||
* Corresponds to the first dataman entry of DM_KEY_FENCE_POINTS and DM_KEY_SAFE_POINTS
|
||||
*/
|
||||
struct mission_stats_entry_s {
|
||||
uint32_t opaque_id; /**< opaque identifier for current stored mission stats */
|
||||
uint16_t num_items; /**< total number of items stored (excluding this one) */
|
||||
uint16_t update_counter; /**< This counter is increased when (some) items change (this can wrap) */
|
||||
uint8_t padding[2];
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -239,6 +240,18 @@ struct DestinationPosition {
|
|||
float yaw; /**< final yaw when landed [rad].*/
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Crc32 mission item struct.
|
||||
* Used to pack relevant mission item ifnromation for us in crc32 mission calculation.
|
||||
*/
|
||||
typedef struct __attribute__((packed)) CrcMissionItem {
|
||||
uint8_t frame;
|
||||
uint16_t command;
|
||||
uint8_t autocontinue;
|
||||
float params[7];
|
||||
} CrcMissionItem_t;
|
||||
|
||||
#if (__GNUC__ >= 5) || __clang__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif // GCC >= 5 || Clang
|
||||
|
|
|
|||
|
|
@ -249,7 +249,7 @@ public:
|
|||
|
||||
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
|
||||
|
||||
int mission_instance_count() const { return _mission_result.mission_update_counter; }
|
||||
int mission_instance_count() const { return _mission_result.mission_id; }
|
||||
|
||||
void set_mission_failure_heading_timeout();
|
||||
|
||||
|
|
|
|||
|
|
@ -171,8 +171,8 @@ void Navigator::run()
|
|||
fds[2].fd = _mission_sub;
|
||||
fds[2].events = POLLIN;
|
||||
|
||||
uint16_t geofence_update_counter{0};
|
||||
uint16_t safe_points_update_counter{0};
|
||||
uint32_t geofence_id{0};
|
||||
uint32_t safe_points_id{0};
|
||||
|
||||
/* rate-limit position subscription to 20 Hz / 50 ms */
|
||||
orb_set_interval(_local_pos_sub, 50);
|
||||
|
|
@ -201,13 +201,13 @@ void Navigator::run()
|
|||
mission_s mission;
|
||||
orb_copy(ORB_ID(mission), _mission_sub, &mission);
|
||||
|
||||
if (mission.geofence_update_counter != geofence_update_counter) {
|
||||
geofence_update_counter = mission.geofence_update_counter;
|
||||
if (mission.geofence_id != geofence_id) {
|
||||
geofence_id = mission.geofence_id;
|
||||
_geofence.updateFence();
|
||||
}
|
||||
|
||||
if (mission.safe_points_update_counter != safe_points_update_counter) {
|
||||
safe_points_update_counter = mission.safe_points_update_counter;
|
||||
if (mission.safe_points_id != safe_points_id) {
|
||||
safe_points_id = mission.safe_points_id;
|
||||
_rtl.updateSafePoints();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -99,9 +99,9 @@ void RTL::updateDatamanCache()
|
|||
_error_state = DatamanState::ReadWait;
|
||||
_dataman_state = DatamanState::Error;
|
||||
|
||||
} else if (_update_counter != _stats.update_counter) {
|
||||
} else if (_opaque_id != _stats.opaque_id) {
|
||||
|
||||
_update_counter = _stats.update_counter;
|
||||
_opaque_id = _stats.opaque_id;
|
||||
_safe_points_updated = false;
|
||||
|
||||
_dataman_cache_safepoint.invalidate();
|
||||
|
|
@ -144,8 +144,8 @@ void RTL::updateDatamanCache()
|
|||
|
||||
}
|
||||
|
||||
if (_mission_counter != _mission_sub.get().mission_update_counter) {
|
||||
_mission_counter = _mission_sub.get().mission_update_counter;
|
||||
if (_mission_id != _mission_sub.get().mission_id) {
|
||||
_mission_id = _mission_sub.get().mission_id;
|
||||
const dm_item_t dm_item = static_cast<dm_item_t>(_mission_sub.get().dataman_id);
|
||||
_dataman_cache_landItem.invalidate();
|
||||
|
||||
|
|
|
|||
|
|
@ -190,13 +190,13 @@ private:
|
|||
|
||||
DatamanState _dataman_state{DatamanState::UpdateRequestWait};
|
||||
DatamanState _error_state{DatamanState::UpdateRequestWait};
|
||||
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated
|
||||
uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated
|
||||
bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache
|
||||
mutable DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4};
|
||||
DatamanClient &_dataman_client_safepoint = _dataman_cache_safepoint.client();
|
||||
bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
|
||||
mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
|
||||
int16_t _mission_counter = -1;
|
||||
uint32_t _mission_id = 0u;
|
||||
|
||||
mission_stats_entry_s _stats;
|
||||
|
||||
|
|
|
|||
|
|
@ -1096,7 +1096,7 @@ DatamanTest::testResetItems()
|
|||
|
||||
mission_stats_entry_s stats;
|
||||
stats.num_items = 0;
|
||||
stats.update_counter = 1;
|
||||
stats.opaque_id = 0;
|
||||
|
||||
success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
|
||||
sizeof(mission_stats_entry_s));
|
||||
|
|
|
|||
Loading…
Reference in New Issue