Add mission point check when update the geofence

[Drone-Lab]

Solved Problem

1. PX4-Autopilot/src/modules/navigator/mission_feasibility_checker.cpp

   Line 108 in c5101c7

   MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid)

   I've found that the MissionFeasibilityChecker() is only triggered correctly when updating a mission. when only updating a geofence,user need to click upload twice to invoke check function. Currently, the geofence update only checks the origin and the current position associated with the geofence, and I've added a check for mission waypoints to make the check on upload more complete.The problem is related to the state machine and asynchronous message reading in geofence.cpp.The checkMissionAgainstGeofence() is written in the mission and the geofence is still being read when the mission has finished uploading and checking.

Fixes #{#22362}
In this bug issue , @czbxzm said ,"1. When swapping the order, uploading the mission waypoint routes first and then plotting the no-fly zones does not trigger any checks, even if there is an overlap between the two;"Actually this check triggers, but requires a second click on upload.(in the version reporting this bug issue)

Solution

1. I've added a check for mission waypoints when update geofence.

Test coverage

test on jmavsim/gz_x500 and QGC

[sfuhrer]

I think this could be fine. It would make you change the mission first if it would validate the planned GF, and then you can also update the GF.

[sfuhrer]

Curious for other inputs though, @dagar @MaEtUgR ?

[KonradRudin]

Hey, i dont think this is the way to go. As you stated, there is already the check here https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_feasibility_checker.cpp#L108-L109 and your new function is basically a reimplementation of it. I know that this function is executed as soon as a mission is uploaded, and thus it could be that a geofence upload is in progress and the mission is not checked with the right geofence, which would result in a wrong evaluation. However, it is checked again before the mission is executed and a geofence violation is caught at that time (though you don't have immediate feedback in QGC). Also to not have a data race by writing and reading to the fence on the same time, there is another PR to add double geofence storage here: #22533. I think the way to go would be to decouple the mission feasibility checker from the mission and run the checks if the home position, geofence, or mission changed, and there is no mavlink mission upload in progress. As a short term fix i would suggest changing the order in which QGC is uploading the mission and make sure that the geofence are uploaded before the mission is uploaded. Further you could call check_mission_valid https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L696 periodically as this checks if the geofence is changed since the last evaluation and runs it again.

[KonradRudin]

Why not reuse the code from https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_feasibility_checker.cpp#L108-L109? This is harder to maintain and currently does not work with non position mission items.

[Drone-Lab]

Why not reuse the code from https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_feasibility_checker.cpp#L108-L109? This is harder to maintain and currently does not work with non position mission items.

I made partial modifications to the code based on reference here.The code here cannot be directly used. This is because it checks using the geofence that has already been successfully uploaded, while we now need to perform pre-upload checks for the geofence.

[Drone-Lab]

Thank you for your advice.

it is checked again before the mission is executed and a geofence violation is caught at that time (though you don't have immediate feedback in QGC).

I don't consider this to be a reasonable solution. And we've had discussions about it before, it could pose issues in certain scenarios.#22394 (comment)

Also to not have a data race by writing and reading to the fence on the same time, there is another PR to add double geofence storage here: #22533.

I agree with you that the drone needs double storage for geofence to separate checking and runing.

Further you could call check_mission_valid https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L696 periodically as this checks if the geofence is changed since the last evaluation and runs it again.

I'm concerned that periodically calling this function might introduce unnecessary overhead.

[KonradRudin]

it is checked again before the mission is executed and a geofence violation is caught at that time (though you don't have immediate feedback in QGC).

I don't consider this to be a reasonable solution. And we've had discussions about it before, it could pose issues in certain scenarios.#22394 (comment)

I agree on this and we should change that, and it is good that you take some initiative here.

Further you could call check_mission_valid https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L696 periodically as this checks if the geofence is changed since the last evaluation and runs it again.

I'm concerned that periodically calling this function might introduce unnecessary overhead.

Yeah it potentially does, But the check if the mission feasibility checker is with the most recent data should be minimal and other overheads might be the Same as your solution, no?
Also for short term fixes, i have no better idea. Your solution runs it drectly on upload which is preferable, but then you should reflect this evaluation in the mission result uorb topic as its done here https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L702-L707 such that it is not run again by the mission feasibility checker. The problem is then, that if e.g. the home position is changed, it is still not run again, up until the mission is started. Also now, it is checked once the geofence is uploaded and once the mission is uploaded, which means if you upload a new mission with new geofence, it is evaluated once on outdated data, depending which one is uploaded first.

I also already have though about how to do the checking properly, but the above problem makes it difficult. i think that QGC always send the mission, geofence, and rally points, irrespective of what you have actually changed. But with mavlink in general you should be able to send only what you actually have changed. Also if you change both and upload them, you would preferably run the check only once after both are uploaded, else you would do the evaluation on outdated data. But there is currently no way of knowing if another upload is pending except maybe waiting for a short amount of time and checking if another mission upload is in progress. Maybe there is also something mission in MAVLINK itself to solve this.

[Drone-Lab]

but then you should reflect this evaluation in the mission result uorb topic as its done here https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L702-L707 such that it is not run again by the mission feasibility checker.

Is the 'reflect' necessary here? I'm not sure about its intended meaning. Currently, this patch does not trigger a recheck of the geofence by the mission feasibility checker.Because if geofence not feasible,it will not be added.

The problem is then, that if e.g. the home position is changed, it is still not run again, up until the mission is started.

We have discussed this topic here.Welcome more advice.#22373

Also now, it is checked once the geofence is uploaded and once the mission is uploaded, which means if you upload a new mission with new geofence, it is evaluated once on outdated data, depending which one is uploaded first.

I don't think this is a problem.The ultimate outcome remains: Illegal missions and geofences will be rejected.

IAlso if you change both and upload them, you would preferably run the check only once after both are uploaded, else you would do the evaluation on outdated data.

I agree with you .As I mentioned, the checkMissionAgainstGeofence() is written in the mission module. I believe this is the root cause.

[KonradRudin]

but then you should reflect this evaluation in the mission result uorb topic as its done here https://github.com/Drone-Lab/PX4-Autopilot/blob/5490f8913bd26ac04d59908271ce9c5e6a1bd414/src/modules/navigator/mission_base.cpp#L702-L707 such that it is not run again by the mission feasibility checker.

Is the 'reflect' necessary here? I'm not sure about its intended meaning. Currently, this patch does not trigger a recheck of the geofence by the mission feasibility checker.Because if geofence not feasible,it will not be added.

Ah, i missed that, so if the mission violates the geofence, the geofence is ignored, correct? The mission feasibility checker makes the opposite, it defines the mission as invalid and wont let you execute the mission. So what i meant with 'reflect' is if the geofence is violated, you should define the mission as invalid as well. So with your approach you would now get the warning in QGC but you could execute the mission and it would fly out of the geofence, if i'm understanding this correctly. That would lead to unsafe behavior.

[Drone-Lab]

So with your approach you would now get the warning in QGC but you could execute the mission and it would fly out of the geofence, if i'm understanding this correctly. That would lead to unsafe behavior.

Don't worry about the unmanned aerial vehicle carrying out the geofence breaching mission, as the mission always undergoes an feasibility check and upload process before the geofence.

[Drone-Lab]

If you are interested in "the mission always before the geofence," you can obtain more details here.

Details

1. When the ground control station upload mission or geofence to the drone.In geofence.cpp, there is a state machine designed to read geofence data from ourb and load it onto the drone, as illustrated in the following picture.

   

   This functionality is called within a while(1) loop in navigator_main.cpp.

   PX4-Autopilot/src/modules/navigator/navigator_main.cpp

   Line 893 in cf840ff

   _geofence.run();

   
   The read and load operations within it utilize asynchronous reading, ensuring that they do not block the function loop.
   

   PX4-Autopilot/src/modules/navigator/geofence.cpp

   Lines 111 to 112 in cf840ff

   	success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
   	sizeof(mission_stats_entry_s));

   
   

   PX4-Autopilot/src/modules/navigator/geofence.cpp

   Lines 155 to 163 in cf840ff

   	case DatamanState::Load:
   	_dataman_cache.update();
   	if (!_dataman_cache.isLoading()) {
   	_dataman_state = DatamanState::UpdateRequestWait;
   	_updateFence();
   	_fence_updated = true;
   	}

2. When the ground control station upload mission or geofence to the drone.In mission_feasibility_checker.cpp, synchronous reading is employed to retrieve updates for the mission, and to check for potential conflicts between the mission and geofence.
   

   PX4-Autopilot/src/modules/navigator/mission_feasibility_checker.cpp

   Lines 122 to 123 in 20129e6

   	bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast<uint8_t *>(&missionitem),
   	sizeof(mission_item_s));

   
   This functionality is called within a while(1) loop in navigator_main.cpp too.
   

   PX4-Autopilot/src/modules/navigator/navigator_main.cpp

   Lines 873 to 877 in 20129e6

   	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
   	if (_navigation_mode_array[i]) {
   	_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
   	}
   	}

3. This is the cause of the issue. When on the ground control station, both the geofence and mission are updated simultaneously. The geofence undergoes multiple state transitions through the state machine (with each state transition requiring the execution of a while(1) loop), and it is only after asynchronous reading that the geofence data can be loaded for the mission_feasibility_checker to examine.
   Therefore, when the mission_feasibility_checker utilizes synchronous reading to obtain mission data, the geofence data involved in the check has not yet been updated. Consequently, the check is performed with outdated geofence data, resulting in the failure of the checker.

[mrpollo]

Hey @KonradRudin can you please check-in with us on this one? we want to help move it forward and have a few ideas

[KonradRudin]

Sorry, was away last week. So basically we need to make sure that the geofence module has already properly loaded the geofence data before we can test for it right?

[Drone-Lab]

Alternatively, add a check after successful upload?

[KonradRudin]

Alternatively, add a check after successful upload?

You mean after successful loading into cache i presume? Yes, as a first step we can force it to check again after successfully loading the geofence.

[Drone-Lab]

what's the further plans? making geofence sync read and adding locks or semaphores?

[KonradRudin]

what's the further plans? making geofence sync read and adding locks or semaphores?

No, the caching is fine. But the geofence should be more a service module as well as send an uorb message if it has loaded the new geofence set.

Maybe for your short term problem we could add a new geofence uorb message, which sends the ID of the loaded geofence. And if this message is updated, other system like the mission feasibility checker know to recheck?

[Drone-Lab]

I agree with your opinion. Do you need me to further modify the pull request according to this requirement?

[KonradRudin]

It depends on your timeline. I also would like to improve the mission feasibility checks a bit and make it independent of the mission, but i won't have time right now. So any help input is appreciated, and this would be a good first incremental step.