forked from Archive/PX4-Autopilot
mavlink_mission: add alternating storage for geofence and safe points on upload
This way the old points are kept on an upload error.
This commit is contained in:
Konrad
Daniel Agar
parent
50f1abaef1
commit
9fd137e88e
|
|
@ -1,5 +1,7 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 mission_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
uint8 fence_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
uint8 safepoint_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
|
||||
uint16 count # count of the missions stored in the dataman
|
||||
int32 current_seq # default -1, start at the one changed latest
|
||||
|
|
|
|||
|
|
@ -58,6 +58,8 @@
|
|||
using matrix::wrap_2pi;
|
||||
|
||||
dm_item_t MavlinkMissionManager::_mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
dm_item_t MavlinkMissionManager::_safepoint_dataman_id = DM_KEY_SAFE_POINTS_0;
|
||||
dm_item_t MavlinkMissionManager::_fence_dataman_id = DM_KEY_FENCE_POINTS_0;
|
||||
bool MavlinkMissionManager::_dataman_init = false;
|
||||
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
|
||||
uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 };
|
||||
|
|
@ -91,6 +93,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
|
|||
}
|
||||
|
||||
_my_mission_dataman_id = _mission_dataman_id;
|
||||
_my_fence_dataman_id = _fence_dataman_id;
|
||||
_my_safepoint_dataman_id = _safepoint_dataman_id;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -116,6 +120,8 @@ MavlinkMissionManager::load_geofence_stats()
|
|||
if (success) {
|
||||
_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
|
||||
_crc32[MAV_MISSION_TYPE_FENCE] = stats.opaque_id;
|
||||
_fence_dataman_id = static_cast<dm_item_t>(stats.dataman_id);
|
||||
_my_fence_dataman_id = _fence_dataman_id;
|
||||
}
|
||||
|
||||
return success;
|
||||
|
|
@ -132,6 +138,8 @@ MavlinkMissionManager::load_safepoint_stats()
|
|||
if (success) {
|
||||
_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
|
||||
_crc32[MAV_MISSION_TYPE_RALLY] = stats.opaque_id;
|
||||
_safepoint_dataman_id = static_cast<dm_item_t>(stats.dataman_id);
|
||||
_my_safepoint_dataman_id = _safepoint_dataman_id;
|
||||
}
|
||||
|
||||
return success;
|
||||
|
|
@ -146,14 +154,16 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1
|
|||
{
|
||||
/* update active mission state */
|
||||
_mission_dataman_id = mission_dataman_id;
|
||||
_my_mission_dataman_id = _mission_dataman_id;
|
||||
_count[MAV_MISSION_TYPE_MISSION] = count;
|
||||
_crc32[MAV_MISSION_TYPE_MISSION] = crc32;
|
||||
_current_seq = seq;
|
||||
_my_mission_dataman_id = _mission_dataman_id;
|
||||
|
||||
mission_s mission{};
|
||||
mission.timestamp = hrt_absolute_time();
|
||||
mission.mission_dataman_id = mission_dataman_id;
|
||||
mission.fence_dataman_id = _fence_dataman_id;
|
||||
mission.safepoint_dataman_id = _safepoint_dataman_id;
|
||||
mission.count = count;
|
||||
mission.current_seq = seq;
|
||||
mission.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
|
||||
|
|
@ -175,11 +185,15 @@ MavlinkMissionManager::update_active_mission(dm_item_t mission_dataman_id, uint1
|
|||
}
|
||||
|
||||
int
|
||||
MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
|
||||
MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32)
|
||||
{
|
||||
_fence_dataman_id = fence_dataman_id;
|
||||
_my_fence_dataman_id = fence_dataman_id;
|
||||
|
||||
mission_stats_entry_s stats;
|
||||
stats.num_items = count;
|
||||
stats.opaque_id = crc32;
|
||||
stats.dataman_id = fence_dataman_id;
|
||||
|
||||
/* update stats in dataman */
|
||||
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
|
||||
|
|
@ -207,11 +221,15 @@ MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
|
|||
}
|
||||
|
||||
int
|
||||
MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
|
||||
MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32)
|
||||
{
|
||||
_safepoint_dataman_id = safepoint_dataman_id;
|
||||
_my_safepoint_dataman_id = safepoint_dataman_id;
|
||||
|
||||
mission_stats_entry_s stats;
|
||||
stats.num_items = count;
|
||||
stats.opaque_id = crc32;
|
||||
stats.dataman_id = safepoint_dataman_id;
|
||||
|
||||
/* update stats in dataman */
|
||||
bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
|
||||
|
|
@ -303,7 +321,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
|||
|
||||
case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
|
||||
mission_fence_point_s mission_fence_point;
|
||||
read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_STATE, seq,
|
||||
read_success = _dataman_client.readSync(_fence_dataman_id, seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
|
||||
|
||||
mission_item.nav_cmd = mission_fence_point.nav_cmd;
|
||||
|
|
@ -323,7 +341,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
|||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
|
||||
read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_item),
|
||||
read_success = _dataman_client.readSync(_safepoint_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_item),
|
||||
sizeof(mission_item_s));
|
||||
}
|
||||
break;
|
||||
|
|
@ -917,11 +935,12 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_FENCE:
|
||||
update_geofence_count(0, 0);
|
||||
update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0, 0, 0);
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY:
|
||||
update_safepoint_count(0, 0);
|
||||
update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 : DM_KEY_SAFE_POINTS_0, 0,
|
||||
0);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -936,13 +955,34 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|||
|
||||
PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
|
||||
|
||||
switch (_mission_type) {
|
||||
case MAV_MISSION_TYPE_MISSION:
|
||||
_transfer_dataman_id = (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_FENCE:
|
||||
_transfer_dataman_id = (_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 :
|
||||
DM_KEY_FENCE_POINTS_0); // use inactive storage for transmission
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY:
|
||||
_transfer_dataman_id = (_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 :
|
||||
DM_KEY_SAFE_POINTS_0); // use inactive storage for transmission
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("mission type %u not handled", _mission_type);
|
||||
_transfer_in_progress = false;
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_INVALID);
|
||||
return;
|
||||
}
|
||||
|
||||
_state = MAVLINK_WPM_STATE_GETLIST;
|
||||
_transfer_seq = 0;
|
||||
_transfer_partner_sysid = msg->sysid;
|
||||
_transfer_partner_compid = msg->compid;
|
||||
_transfer_count = wpc.count;
|
||||
_transfer_mission_dataman_id = (_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
|
||||
_transfer_current_seq = -1;
|
||||
_transfer_land_start_marker = -1;
|
||||
_transfer_land_marker = -1;
|
||||
|
|
@ -1099,7 +1139,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
|
||||
} else {
|
||||
|
||||
write_failed = !_dataman_client.writeSync(_transfer_mission_dataman_id, wp.seq,
|
||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_item),
|
||||
sizeof(struct mission_item_s));
|
||||
|
||||
|
|
@ -1142,7 +1182,6 @@ 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, 0);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -1152,7 +1191,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
mission_fence_point.frame = mission_item.frame;
|
||||
|
||||
if (!check_failed) {
|
||||
write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, wp.seq,
|
||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
|
||||
}
|
||||
|
||||
|
|
@ -1160,7 +1199,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
|
||||
write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS_0, wp.seq,
|
||||
write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_item), sizeof(mission_item_s), 2_s);
|
||||
}
|
||||
break;
|
||||
|
|
@ -1173,7 +1212,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
if (write_failed || check_failed) {
|
||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_mission_dataman_id);
|
||||
PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id);
|
||||
|
||||
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
||||
|
||||
|
|
@ -1208,15 +1247,30 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||
case MAV_MISSION_TYPE_MISSION:
|
||||
_land_start_marker = _transfer_land_start_marker;
|
||||
_land_marker = _transfer_land_marker;
|
||||
update_active_mission(_transfer_mission_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
|
||||
|
||||
// Only need to update if the mission actually changed
|
||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) {
|
||||
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, _transfer_current_crc32);
|
||||
|
||||
// Only need to update if the mission actually changed
|
||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) {
|
||||
ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY:
|
||||
ret = update_safepoint_count(_transfer_count, _transfer_current_crc32);
|
||||
|
||||
// Only need to update if the mission actually changed
|
||||
if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) {
|
||||
ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -1269,11 +1323,13 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_FENCE:
|
||||
ret = update_geofence_count(0, 0);
|
||||
ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0,
|
||||
0, 0);
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_RALLY:
|
||||
ret = update_safepoint_count(0, 0);
|
||||
ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 :
|
||||
DM_KEY_SAFE_POINTS_0, 0, 0);
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_ALL:
|
||||
|
|
@ -1281,8 +1337,10 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||
_land_marker = -1;
|
||||
update_active_mission(_mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
|
||||
ret = update_geofence_count(0, 0);
|
||||
ret = update_safepoint_count(0, 0) || ret;
|
||||
ret = update_geofence_count(_fence_dataman_id == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0,
|
||||
0, 0);
|
||||
ret = update_safepoint_count(_safepoint_dataman_id == DM_KEY_SAFE_POINTS_0 ? DM_KEY_SAFE_POINTS_1 :
|
||||
DM_KEY_SAFE_POINTS_0, 0, 0) || ret;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -1785,11 +1843,13 @@ void MavlinkMissionManager::check_active_mission()
|
|||
if (_mission_sub.updated()) {
|
||||
_mission_sub.update();
|
||||
|
||||
if (_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE]) {
|
||||
if ((_mission_sub.get().geofence_id != _crc32[MAV_MISSION_TYPE_FENCE])
|
||||
|| (_my_fence_dataman_id != (dm_item_t) _mission_sub.get().fence_dataman_id)) {
|
||||
load_geofence_stats();
|
||||
}
|
||||
|
||||
if (_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY]) {
|
||||
if ((_mission_sub.get().safe_points_id != _crc32[MAV_MISSION_TYPE_RALLY])
|
||||
|| (_my_safepoint_dataman_id != (dm_item_t) _mission_sub.get().safepoint_dataman_id)) {
|
||||
load_safepoint_stats();
|
||||
}
|
||||
|
||||
|
|
@ -1797,7 +1857,6 @@ void MavlinkMissionManager::check_active_mission()
|
|||
|| (_my_mission_dataman_id != (dm_item_t)_mission_sub.get().mission_dataman_id)) {
|
||||
PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
|
||||
init_offboard_mission(_mission_sub.get());
|
||||
_my_mission_dataman_id = _mission_dataman_id;
|
||||
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION],
|
||||
MAV_MISSION_TYPE_MISSION, _crc32[MAV_MISSION_TYPE_MISSION]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -107,7 +107,11 @@ private:
|
|||
unsigned _filesystem_errcount{0}; ///< File system error count
|
||||
|
||||
static dm_item_t _mission_dataman_id; ///< Global Dataman storage ID for active mission
|
||||
static dm_item_t _safepoint_dataman_id; ///< Global dataman storage id for active safepoints
|
||||
static dm_item_t _fence_dataman_id; ///< Global dataman storage id for active geofence
|
||||
dm_item_t _my_mission_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID for mission
|
||||
dm_item_t _my_safepoint_dataman_id{DM_KEY_SAFE_POINTS_0}; ///< class Dataman storage ID for safepoints
|
||||
dm_item_t _my_fence_dataman_id{DM_KEY_FENCE_POINTS_0}; ///< class Dataman storage ID for geofence
|
||||
|
||||
static bool _dataman_init; ///< Dataman initialized
|
||||
|
||||
|
|
@ -117,7 +121,7 @@ private:
|
|||
|
||||
int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached)
|
||||
|
||||
dm_item_t _transfer_mission_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current mission transmission
|
||||
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
|
||||
|
|
@ -172,10 +176,10 @@ private:
|
|||
bool write_to_dataman = true);
|
||||
|
||||
/** store the geofence count to dataman */
|
||||
int update_geofence_count(unsigned count, uint32_t crc32);
|
||||
int update_geofence_count(dm_item_t fence_dataman_id, unsigned count, uint32_t crc32);
|
||||
|
||||
/** store the safepoint count to dataman */
|
||||
int update_safepoint_count(unsigned count, uint32_t crc32);
|
||||
int update_safepoint_count(dm_item_t safepoint_dataman_id, unsigned count, uint32_t crc32);
|
||||
|
||||
/** load geofence stats from dataman */
|
||||
bool load_geofence_stats();
|
||||
|
|
|
|||
|
|
@ -140,7 +140,7 @@ void Geofence::run()
|
|||
}
|
||||
|
||||
for (int index = 0; index < _dataman_cache.size(); ++index) {
|
||||
_dataman_cache.load(DM_KEY_FENCE_POINTS_0, index);
|
||||
_dataman_cache.load(static_cast<dm_item_t>(_stats.dataman_id), index);
|
||||
}
|
||||
|
||||
_dataman_state = DatamanState::Load;
|
||||
|
|
@ -191,7 +191,7 @@ void Geofence::_updateFence()
|
|||
|
||||
while (current_seq < _dataman_cache.size()) {
|
||||
|
||||
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, current_seq,
|
||||
bool success = _dataman_cache.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
sizeof(mission_fence_point_s));
|
||||
|
||||
|
|
@ -412,14 +412,15 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
|
|||
|
||||
for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
|
||||
|
||||
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + i,
|
||||
dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
|
||||
bool success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + i,
|
||||
reinterpret_cast<uint8_t *>(&temp_vertex_i), sizeof(mission_fence_point_s));
|
||||
|
||||
if (!success) {
|
||||
break;
|
||||
}
|
||||
|
||||
success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index + j,
|
||||
success = _dataman_cache.loadWait(fence_dataman_id, polygon.dataman_index + j,
|
||||
reinterpret_cast<uint8_t *>(&temp_vertex_j), sizeof(mission_fence_point_s));
|
||||
|
||||
if (!success) {
|
||||
|
|
@ -448,7 +449,7 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
|
|||
{
|
||||
|
||||
mission_fence_point_s circle_point{};
|
||||
bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS_0, polygon.dataman_index,
|
||||
bool success = _dataman_cache.loadWait(static_cast<dm_item_t>(_stats.dataman_id), polygon.dataman_index,
|
||||
reinterpret_cast<uint8_t *>(&circle_point), sizeof(mission_fence_point_s));
|
||||
|
||||
if (!success) {
|
||||
|
|
@ -491,8 +492,18 @@ Geofence::loadFromFile(const char *filename)
|
|||
const char commentChar = '#';
|
||||
int ret_val = PX4_ERROR;
|
||||
|
||||
/* Make sure no data is left in the datamanager */
|
||||
clearDm();
|
||||
mission_stats_entry_s stat;
|
||||
{
|
||||
const bool success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stat),
|
||||
sizeof(mission_stats_entry_s));
|
||||
|
||||
if (!success) {
|
||||
PX4_ERR("Could not read fence dataman state");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
dm_item_t write_fence_dataman_id{static_cast<dm_item_t>(stat.dataman_id) == DM_KEY_FENCE_POINTS_0 ? DM_KEY_FENCE_POINTS_1 : DM_KEY_FENCE_POINTS_0};
|
||||
|
||||
/* open the mixer definition file */
|
||||
fp = fopen(GEOFENCE_FILENAME, "r");
|
||||
|
|
@ -554,7 +565,7 @@ Geofence::loadFromFile(const char *filename)
|
|||
}
|
||||
}
|
||||
|
||||
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, pointCounter, reinterpret_cast<uint8_t *>(&vertex),
|
||||
bool success = _dataman_client.writeSync(write_fence_dataman_id, pointCounter, reinterpret_cast<uint8_t *>(&vertex),
|
||||
sizeof(vertex));
|
||||
|
||||
if (!success) {
|
||||
|
|
@ -588,13 +599,13 @@ Geofence::loadFromFile(const char *filename)
|
|||
for (int seq = 0; seq < pointCounter; ++seq) {
|
||||
mission_fence_point_s mission_fence_point;
|
||||
|
||||
bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
bool success = _dataman_client.readSync(write_fence_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
sizeof(mission_fence_point_s));
|
||||
|
||||
if (success) {
|
||||
mission_fence_point.vertex_count = pointCounter;
|
||||
crc32 = crc32_for_fence_point(mission_fence_point, crc32);
|
||||
_dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
_dataman_client.writeSync(write_fence_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
|
||||
sizeof(mission_fence_point_s));
|
||||
}
|
||||
}
|
||||
|
|
@ -622,13 +633,6 @@ error:
|
|||
return ret_val;
|
||||
}
|
||||
|
||||
int Geofence::clearDm()
|
||||
{
|
||||
_dataman_client.clearSync(DM_KEY_FENCE_POINTS_STATE);
|
||||
updateFence();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
bool Geofence::isHomeRequired()
|
||||
{
|
||||
bool max_horizontal_enabled = (_param_gf_max_hor_dist.get() > FLT_EPSILON);
|
||||
|
|
|
|||
|
|
@ -108,8 +108,6 @@ public:
|
|||
|
||||
virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude);
|
||||
|
||||
int clearDm();
|
||||
|
||||
bool valid();
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -205,7 +205,8 @@ struct mission_item_s {
|
|||
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) */
|
||||
uint8_t padding[2];
|
||||
uint8_t dataman_id; /**< dm_item_t storage place*/
|
||||
uint8_t padding[1];
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ void RTL::updateDatamanCache()
|
|||
}
|
||||
|
||||
for (int index = 0; index < _dataman_cache_safepoint.size(); ++index) {
|
||||
_dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS_0, index);
|
||||
_dataman_cache_safepoint.load(static_cast<dm_item_t>(_stats.dataman_id), index);
|
||||
}
|
||||
|
||||
_dataman_state = DatamanState::Load;
|
||||
|
|
@ -397,7 +397,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosit
|
|||
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
|
||||
mission_item_s mission_safe_point;
|
||||
|
||||
bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS_0, current_seq,
|
||||
bool success = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_safe_point),
|
||||
sizeof(mission_item_s), 500_ms);
|
||||
|
||||
|
|
@ -629,7 +629,7 @@ land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position)
|
|||
for (int current_seq = 0; current_seq < _stats.num_items; ++current_seq) {
|
||||
mission_item_s mission_item{};
|
||||
|
||||
bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS_0, current_seq,
|
||||
bool success_mission_item = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
|
||||
reinterpret_cast<uint8_t *>(&mission_item),
|
||||
sizeof(mission_item_s));
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue