mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-05 14:17:20 +08:00
mision: only run mission feasibility checks when mission updated
Instead of also when geofence/safe points updated. This prevents reporting multiple times.
This commit is contained in:
@@ -4,5 +4,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
|
uint16 count # count of the missions stored in the dataman
|
||||||
int32 current_seq # default -1, start at the one changed latest
|
int32 current_seq # default -1, start at the one changed latest
|
||||||
|
|
||||||
|
int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased
|
||||||
int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
|
int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased
|
||||||
int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased
|
int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased
|
||||||
|
|||||||
@@ -62,6 +62,7 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
|
|||||||
int32_t MavlinkMissionManager::_current_seq = 0;
|
int32_t MavlinkMissionManager::_current_seq = 0;
|
||||||
bool MavlinkMissionManager::_transfer_in_progress = false;
|
bool MavlinkMissionManager::_transfer_in_progress = false;
|
||||||
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
|
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
|
||||||
|
uint16_t MavlinkMissionManager::_mission_update_counter = 0;
|
||||||
uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
|
uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
|
||||||
uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
|
uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
|
||||||
|
|
||||||
@@ -146,6 +147,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
|
|||||||
mission.dataman_id = dataman_id;
|
mission.dataman_id = dataman_id;
|
||||||
mission.count = count;
|
mission.count = count;
|
||||||
mission.current_seq = seq;
|
mission.current_seq = seq;
|
||||||
|
mission.mission_update_counter = _mission_update_counter;
|
||||||
mission.geofence_update_counter = _geofence_update_counter;
|
mission.geofence_update_counter = _geofence_update_counter;
|
||||||
mission.safe_points_update_counter = _safepoint_update_counter;
|
mission.safe_points_update_counter = _safepoint_update_counter;
|
||||||
|
|
||||||
@@ -869,6 +871,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|||||||
switch (_mission_type) {
|
switch (_mission_type) {
|
||||||
case MAV_MISSION_TYPE_MISSION:
|
case MAV_MISSION_TYPE_MISSION:
|
||||||
|
|
||||||
|
++_mission_update_counter;
|
||||||
|
|
||||||
/* alternate dataman ID anyway to let navigator know about changes */
|
/* alternate dataman ID anyway to let navigator know about changes */
|
||||||
|
|
||||||
if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
|
if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
|
||||||
@@ -1155,6 +1159,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|||||||
|
|
||||||
switch (_mission_type) {
|
switch (_mission_type) {
|
||||||
case MAV_MISSION_TYPE_MISSION:
|
case MAV_MISSION_TYPE_MISSION:
|
||||||
|
++_mission_update_counter;
|
||||||
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
|
update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -1209,6 +1214,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||||||
|
|
||||||
switch (wpca.mission_type) {
|
switch (wpca.mission_type) {
|
||||||
case MAV_MISSION_TYPE_MISSION:
|
case MAV_MISSION_TYPE_MISSION:
|
||||||
|
++_mission_update_counter;
|
||||||
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_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);
|
||||||
break;
|
break;
|
||||||
@@ -1222,6 +1228,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case MAV_MISSION_TYPE_ALL:
|
case MAV_MISSION_TYPE_ALL:
|
||||||
|
++_mission_update_counter;
|
||||||
update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_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);
|
||||||
ret = update_geofence_count(0);
|
ret = update_geofence_count(0);
|
||||||
|
|||||||
@@ -132,6 +132,7 @@ private:
|
|||||||
|
|
||||||
uORB::Publication<mission_s> _offboard_mission_pub{ORB_ID(mission)};
|
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 _geofence_update_counter;
|
||||||
static uint16_t _safepoint_update_counter;
|
static uint16_t _safepoint_update_counter;
|
||||||
|
|
||||||
|
|||||||
@@ -624,6 +624,7 @@ Mission::update_mission()
|
|||||||
/* otherwise, just leave it */
|
/* otherwise, just leave it */
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (old_mission.mission_update_counter != _mission.mission_update_counter) {
|
||||||
check_mission_valid(true);
|
check_mission_valid(true);
|
||||||
|
|
||||||
failed = !_navigator->get_mission_result()->valid;
|
failed = !_navigator->get_mission_result()->valid;
|
||||||
@@ -648,6 +649,7 @@ Mission::update_mission()
|
|||||||
!_navigator->get_land_detected()->landed) {
|
!_navigator->get_land_detected()->landed) {
|
||||||
_mission_waypoints_changed = true;
|
_mission_waypoints_changed = true;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_ERR("mission update failed");
|
PX4_ERR("mission update failed");
|
||||||
|
|||||||
Reference in New Issue
Block a user