forked from Archive/PX4-Autopilot
Navigator: Prevent busy-looping if Dataman read/write times out
MissionBase did not initialize its mission data, thus could enter an infinite loop in updateDatamanCache() if the initMission() failed to read the mission off, for example, due to the SDCard storage task taking longer than the timeout to respond. This change constrains the loading loop and resets the mission data even if the data write failed.
This commit is contained in:
parent
8243b4f474
commit
643d3e3bf3
|
@ -89,10 +89,9 @@ MissionBase::updateDatamanCache()
|
||||||
{
|
{
|
||||||
if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) {
|
if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) {
|
||||||
|
|
||||||
int32_t start_index = _mission.current_seq;
|
const int32_t start_index = math::constrain(_mission.current_seq, INT32_C(0), int32_t(_mission.count) - 1);
|
||||||
int32_t end_index = start_index + _dataman_cache_size_signed;
|
const int32_t end_index = math::constrain(start_index + _dataman_cache_size_signed, INT32_C(0),
|
||||||
|
int32_t(_mission.count) - 1);
|
||||||
end_index = math::max(math::min(end_index, static_cast<int32_t>(_mission.count)), INT32_C(0));
|
|
||||||
|
|
||||||
for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) {
|
for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) {
|
||||||
|
|
||||||
|
@ -115,8 +114,8 @@ void MissionBase::updateMavlinkMission()
|
||||||
const bool mission_data_changed = checkMissionDataChanged(new_mission);
|
const bool mission_data_changed = checkMissionDataChanged(new_mission);
|
||||||
|
|
||||||
if (new_mission.current_seq < 0) {
|
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),
|
new_mission.current_seq = math::constrain(_mission.current_seq, INT32_C(0),
|
||||||
INT32_C(0));
|
static_cast<int32_t>(new_mission.count) - 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
_mission = new_mission;
|
_mission = new_mission;
|
||||||
|
@ -1147,22 +1146,20 @@ void MissionBase::resetMission()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Set a new mission*/
|
/* Set a new mission*/
|
||||||
mission_s new_mission{_mission};
|
_mission.timestamp = hrt_absolute_time();
|
||||||
new_mission.timestamp = hrt_absolute_time();
|
_mission.current_seq = 0;
|
||||||
new_mission.current_seq = 0;
|
_mission.land_start_index = -1;
|
||||||
new_mission.land_start_index = -1;
|
_mission.land_index = -1;
|
||||||
new_mission.land_index = -1;
|
_mission.count = 0u;
|
||||||
new_mission.count = 0u;
|
_mission.mission_id = 0u;
|
||||||
new_mission.mission_id = 0u;
|
_mission.mission_dataman_id = _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ?
|
||||||
new_mission.mission_dataman_id = _mission.mission_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ?
|
DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
||||||
DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||||
DM_KEY_WAYPOINTS_OFFBOARD_0;
|
|
||||||
|
|
||||||
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&new_mission),
|
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&_mission),
|
||||||
sizeof(mission_s));
|
sizeof(mission_s));
|
||||||
|
|
||||||
if (success) {
|
if (success) {
|
||||||
_mission = new_mission;
|
|
||||||
_mission_pub.publish(_mission);
|
_mission_pub.publish(_mission);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
Loading…
Reference in New Issue