Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add mission point check when update the geofence #22531

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Add mission point check when update the geofence #22531

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1c8e55a
Add mission point check when update the geofence
BladeY1 Dec 13, 2023
821d48a
Update geofence.cpp
Drone-Lab Dec 24, 2023
5490f89
add message about violated waypoint
Drone-Lab Dec 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 26 additions & 1 deletion src/modules/navigator/geofence.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,8 +258,11 @@ void Geofence::_updateFence()
// check if current position is inside the fence and vehicle is armed
const bool current_position_check_okay = checkCurrentPositionRequirementsForGeofence(polygon);

//check if current mission point inside the geofence
const bool current_mission_check_okay = checkMissionRequirementsForGeofence(polygon);

// discard the polygon if at least one check fails by not incrementing the counter in that case
if (home_check_okay && current_position_check_okay) {
if (home_check_okay && current_position_check_okay && current_mission_check_okay) {
++_num_polygons;

}
Expand All @@ -275,6 +278,28 @@ void Geofence::_updateFence()
}
}

bool Geofence::checkMissionRequirementsForGeofence(const PolygonInfo &polygon)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

{
mission_s mission;
_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),sizeof(mission_s));
bool checks_pass = false;
//check all mission against all geofence
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
_dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast<uint8_t *>(&missionitem),
sizeof(mission_item_s));
//missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
checks_pass = checkPointAgainstPolygonCircle(polygon,missionitem.lat, missionitem.lon, missionitem.altitude);
if (!checks_pass) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence invalid, against mission waypoint %zu\t",i + 1);
events::send<int16_t>(events::ID("navigator_geofence_invalid_against_mission"), {events::Log::Critical, events::LogInternal::Warning},
"Geofence invalid, against mission waypoint {1} ",i + 1);
break;
}
}
return checks_pass;
}

bool Geofence::checkHomeRequirementsForGeofence(const PolygonInfo &polygon)
{
bool checks_pass = true;
Expand Down
6 changes: 6 additions & 0 deletions src/modules/navigator/geofence.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,12 @@ class Geofence : public ModuleParams
*/
bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude);

/**
* Check polygon or circle geofence fullfills the requirements relative to the current mission.
* @return true if checks pass
*/
bool checkMissionRequirementsForGeofence(const PolygonInfo &polygon);

/**
* Check if a single point is within a polygon or circle
* @return true if within polygon or circle
Expand Down