forked from Archive/PX4-Autopilot
Compare commits
7 Commits
main
...
pr-restore
Author | SHA1 | Date |
---|---|---|
Igor Mišić | ce301a17ea | |
Igor Mišić | 37ddd095ad | |
Igor Mišić | 2877e74021 | |
Igor Mišić | bb6bf89913 | |
Igor Mišić | f95c899d18 | |
Igor Mišić | 11f0c5a3e6 | |
Igor Mišić | 819c771da1 |
|
@ -3,3 +3,6 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam
|
|||
|
||||
uint16 count # count of the missions stored in the dataman
|
||||
int32 current_seq # default -1, start at the one changed latest
|
||||
|
||||
bool valid # true if mission passed feasibility checks
|
||||
bool clear_mission # true if mission is cleared in dataman
|
|
@ -148,7 +148,7 @@ MavlinkMissionManager::load_safepoint_stats()
|
|||
* Publish mission topic to notify navigator about changes.
|
||||
*/
|
||||
int
|
||||
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
|
||||
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission)
|
||||
{
|
||||
// We want to make sure the whole struct is initialized including padding before getting written by dataman.
|
||||
mission_s mission{};
|
||||
|
@ -156,6 +156,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
|
|||
mission.dataman_id = dataman_id;
|
||||
mission.count = count;
|
||||
mission.current_seq = seq;
|
||||
mission.clear_mission = clear_mission;
|
||||
|
||||
/* update mission state in dataman */
|
||||
|
||||
|
@ -486,6 +487,12 @@ MavlinkMissionManager::send()
|
|||
|
||||
if (_mission_result_sub.update(&mission_result)) {
|
||||
|
||||
/* If an infeasible mission is uploaded and the old feasible mission exists item count could be wrong.
|
||||
* This is restoring item count from the feasible mission. */
|
||||
if (_count[MAV_MISSION_TYPE_MISSION] != mission_result.seq_total) {
|
||||
_count[MAV_MISSION_TYPE_MISSION] = mission_result.seq_total;
|
||||
}
|
||||
|
||||
if (_current_seq != mission_result.seq_current) {
|
||||
_current_seq = mission_result.seq_current;
|
||||
|
||||
|
@ -943,8 +950,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|||
_transfer_partner_sysid = msg->sysid;
|
||||
_transfer_partner_compid = msg->compid;
|
||||
_transfer_count = wpc.count;
|
||||
_transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
|
||||
_transfer_dataman_id = next_transfer_dataman_id();
|
||||
_transfer_current_seq = -1;
|
||||
|
||||
if (_mission_type == MAV_MISSION_TYPE_FENCE) {
|
||||
|
@ -994,6 +1000,42 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
dm_item_t
|
||||
MavlinkMissionManager::next_transfer_dataman_id()
|
||||
{
|
||||
dm_item_t transfer_dataman_id = _dataman_id;
|
||||
/* lock MISSION_STATE item */
|
||||
int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
|
||||
|
||||
if (dm_lock_ret != 0) {
|
||||
PX4_ERR("DM_KEY_MISSION_STATE lock failed");
|
||||
}
|
||||
|
||||
mission_s mission_state;
|
||||
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
|
||||
|
||||
/* unlock MISSION_STATE item */
|
||||
if (dm_lock_ret == 0) {
|
||||
dm_unlock(DM_KEY_MISSION_STATE);
|
||||
}
|
||||
|
||||
if (ret == sizeof(mission_s)) {
|
||||
_dataman_id = (dm_item_t)mission_state.dataman_id;
|
||||
transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
|
||||
|
||||
} else if (ret == 0) {
|
||||
//dataman is empty (new or formatted SD card)
|
||||
transfer_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
} else {
|
||||
PX4_ERR("Dataman can't read DM_KEY_MISSION_STATE. The actual size of readout is %d, expected size is %d.", ret,
|
||||
static_cast<int>(sizeof(mission_s)));
|
||||
}
|
||||
|
||||
return transfer_dataman_id;
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkMissionManager::switch_to_idle_state()
|
||||
{
|
||||
|
@ -1273,7 +1315,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||
switch (wpca.mission_type) {
|
||||
case MAV_MISSION_TYPE_MISSION:
|
||||
ret = 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, true);
|
||||
break;
|
||||
|
||||
case MAV_MISSION_TYPE_FENCE:
|
||||
|
@ -1286,7 +1328,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||
|
||||
case MAV_MISSION_TYPE_ALL:
|
||||
ret = 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, true);
|
||||
ret = update_geofence_count(0) || ret;
|
||||
ret = update_safepoint_count(0) || ret;
|
||||
break;
|
||||
|
|
|
@ -159,7 +159,7 @@ private:
|
|||
|
||||
void init_offboard_mission();
|
||||
|
||||
int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
|
||||
int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission = false);
|
||||
|
||||
/** store the geofence count to dataman */
|
||||
int update_geofence_count(unsigned count);
|
||||
|
@ -216,6 +216,15 @@ private:
|
|||
|
||||
void handle_mission_count(const mavlink_message_t *msg);
|
||||
|
||||
/**
|
||||
* Helper function to determine next transfer dataman id.
|
||||
* It reads DM_KEY_MISSION_STATE which stores previous valid dataman_id.
|
||||
* Since mavlink_mission module and mission module communicate over dataman,
|
||||
* mission module is restoring dataman_id for invalid mission to previous valid mission
|
||||
* if valid mission exist.
|
||||
*/
|
||||
dm_item_t next_transfer_dataman_id();
|
||||
|
||||
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);
|
||||
|
|
|
@ -482,56 +482,70 @@ void
|
|||
Mission::update_mission()
|
||||
{
|
||||
|
||||
bool failed = true;
|
||||
bool failed = false;
|
||||
|
||||
/* Reset vehicle_roi
|
||||
* Missions that do not explicitly configure ROI would not override
|
||||
* an existing ROI setting from previous missions */
|
||||
_navigator->reset_vroi();
|
||||
|
||||
const mission_s old_mission = _mission;
|
||||
_old_mission = _mission;
|
||||
_old_mission.current_seq = _current_mission_index;
|
||||
|
||||
if (_mission_sub.copy(&_mission)) {
|
||||
/* determine current index */
|
||||
if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
|
||||
_current_mission_index = _mission.current_seq;
|
||||
|
||||
/* If _mission.clear_mission flag is set to true, mission is deleted from dataman.
|
||||
* No mission validation needed. */
|
||||
if (_mission.clear_mission) {
|
||||
_mission.count = 0;
|
||||
_mission.current_seq = 0;
|
||||
_current_mission_index = 0;
|
||||
_mission.valid = false;
|
||||
|
||||
} else {
|
||||
/* if less items available, reset to first item */
|
||||
if (_current_mission_index >= (int)_mission.count) {
|
||||
_current_mission_index = 0;
|
||||
|
||||
} else if (_current_mission_index < 0) {
|
||||
/* if not initialized, set it to 0 */
|
||||
_current_mission_index = 0;
|
||||
/* determine current index */
|
||||
if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
|
||||
_current_mission_index = _mission.current_seq;
|
||||
|
||||
} else {
|
||||
/* if less items available, reset to first item */
|
||||
if (_current_mission_index >= (int)_mission.count) {
|
||||
_current_mission_index = 0;
|
||||
|
||||
} else if (_current_mission_index < 0) {
|
||||
/* if not initialized, set it to 0 */
|
||||
_current_mission_index = 0;
|
||||
}
|
||||
|
||||
/* otherwise, just leave it */
|
||||
}
|
||||
|
||||
/* otherwise, just leave it */
|
||||
}
|
||||
check_mission_valid(true);
|
||||
|
||||
check_mission_valid(true);
|
||||
failed = !_navigator->get_mission_result()->valid;
|
||||
|
||||
failed = !_navigator->get_mission_result()->valid;
|
||||
if (!failed) {
|
||||
/* reset mission failure if we have an updated valid mission */
|
||||
_navigator->get_mission_result()->failure = false;
|
||||
|
||||
if (!failed) {
|
||||
/* reset mission failure if we have an updated valid mission */
|
||||
_navigator->get_mission_result()->failure = false;
|
||||
/* reset sequence info as well */
|
||||
_navigator->get_mission_result()->seq_reached = -1;
|
||||
_navigator->get_mission_result()->seq_total = _mission.count;
|
||||
|
||||
/* reset sequence info as well */
|
||||
_navigator->get_mission_result()->seq_reached = -1;
|
||||
_navigator->get_mission_result()->seq_total = _mission.count;
|
||||
/* reset work item if new mission has been accepted */
|
||||
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
|
||||
_mission_changed = true;
|
||||
}
|
||||
|
||||
/* reset work item if new mission has been accepted */
|
||||
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
|
||||
_mission_changed = true;
|
||||
}
|
||||
/* check if the mission waypoints changed while the vehicle is in air
|
||||
* TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
|
||||
if (((_mission.count != _old_mission.count) ||
|
||||
(_mission.dataman_id != _old_mission.dataman_id)) &&
|
||||
!_navigator->get_land_detected()->landed) {
|
||||
_mission_waypoints_changed = true;
|
||||
}
|
||||
|
||||
/* check if the mission waypoints changed while the vehicle is in air
|
||||
* TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
|
||||
if (((_mission.count != old_mission.count) ||
|
||||
(_mission.dataman_id != old_mission.dataman_id)) &&
|
||||
!_navigator->get_land_detected()->landed) {
|
||||
_mission_waypoints_changed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
@ -544,10 +558,15 @@ Mission::update_mission()
|
|||
PX4_WARN("mission check failed");
|
||||
}
|
||||
|
||||
// reset the mission
|
||||
_mission.count = 0;
|
||||
_mission.current_seq = 0;
|
||||
_current_mission_index = 0;
|
||||
bool restored = restore_old_mission();
|
||||
|
||||
if (!restored) {
|
||||
|
||||
// Mission can't be restored, reset the mission
|
||||
_mission.count = 0;
|
||||
_mission.current_seq = 0;
|
||||
_current_mission_index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// find and store landing start marker (if available)
|
||||
|
@ -556,6 +575,68 @@ Mission::update_mission()
|
|||
set_current_mission_item();
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::restore_old_mission()
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
_mission = _old_mission;
|
||||
|
||||
if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) &&
|
||||
_mission.valid) {
|
||||
|
||||
/* if armed and valid don't check old mission validity again but update navigator */
|
||||
_navigator->get_mission_result()->valid = true;
|
||||
_navigator->get_mission_result()->seq_total = _mission.count;
|
||||
_navigator->increment_mission_instance_count();
|
||||
_navigator->set_mission_result_updated();
|
||||
_home_inited = _navigator->home_position_valid();
|
||||
find_mission_land_start();
|
||||
|
||||
} else {
|
||||
/* if not armed check validity again in case we changed feasibility parameters */
|
||||
check_mission_valid(true);
|
||||
}
|
||||
|
||||
if (_mission.valid) {
|
||||
_mission_changed = true;
|
||||
_navigator->get_mission_result()->failure = false;
|
||||
|
||||
/* For completed old mission reset sequence to the first item. */
|
||||
if (_old_mission.current_seq >= _old_mission.count) {
|
||||
_current_mission_index = 0;
|
||||
|
||||
} else {
|
||||
_current_mission_index = _old_mission.current_seq;
|
||||
}
|
||||
|
||||
/* lock MISSION_STATE item */
|
||||
int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
|
||||
|
||||
if (dm_lock_ret != 0) {
|
||||
PX4_ERR("DM_KEY_MISSION_STATE lock failed");
|
||||
}
|
||||
|
||||
/* Since mission is invalid restore DM_KEY_MISSION_STATE with old dataman_id.
|
||||
* This helps mavlink_mission module to not overwrite the valid mission if two or more invalid missions are sent.
|
||||
*/
|
||||
int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &_mission, sizeof(mission_s));
|
||||
|
||||
/* unlock MISSION_STATE item */
|
||||
if (dm_lock_ret == 0) {
|
||||
dm_unlock(DM_KEY_MISSION_STATE);
|
||||
}
|
||||
|
||||
if (res == sizeof(mission_s)) {
|
||||
success = true;
|
||||
|
||||
} else {
|
||||
PX4_ERR("Can't write DM_KEY_MISSION_STATE. The next invalid mission will not be restored!");
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::advance_mission()
|
||||
|
@ -1717,6 +1798,7 @@ Mission::check_mission_valid(bool force)
|
|||
_param_mis_dist_wps.get(),
|
||||
_navigator->mission_landing_required());
|
||||
|
||||
_mission.valid = _navigator->get_mission_result()->valid;
|
||||
_navigator->get_mission_result()->seq_total = _mission.count;
|
||||
_navigator->increment_mission_instance_count();
|
||||
_navigator->set_mission_result_updated();
|
||||
|
|
|
@ -110,6 +110,11 @@ private:
|
|||
*/
|
||||
void update_mission();
|
||||
|
||||
/**
|
||||
* Restore old mission if update mission was not successful
|
||||
*/
|
||||
bool restore_old_mission();
|
||||
|
||||
/**
|
||||
* Move on to next mission item or switch to loiter
|
||||
*/
|
||||
|
@ -247,6 +252,7 @@ private:
|
|||
|
||||
uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */
|
||||
mission_s _mission {};
|
||||
mission_s _old_mission {}; /**< old mission is used as a backup and for comparison with a new mission */
|
||||
|
||||
int32_t _current_mission_index{-1};
|
||||
|
||||
|
|
Loading…
Reference in New Issue