dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: Geofence: always run geofence_breach_check(), not only only on pos update 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-11-16 15:12:00 +01:00 committed by Daniel Agar
parent ade68aa563
commit e280a6c6e6
2 changed files with 13 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/navigator/navigator.h
View File

@ -260,7 +260,7 @@ public:
bool abort_landing();
void geofence_breach_check(bool &have_geofence_position_data);
void geofence_breach_check();
// Param access
int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); }

54
src/modules/navigator/navigator_main.cpp
View File

@ -148,7 +148,6 @@ void Navigator::params_update()
void Navigator::run()
{
bool have_geofence_position_data = false;
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file */
@ -216,19 +215,11 @@ void Navigator::run()
/* gps updated */
if (_gps_pos_sub.updated()) {
_gps_pos_sub.copy(&_gps_pos);
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
}
/* global position updated */
if (_global_pos_sub.updated()) {
_global_pos_sub.copy(&_global_pos);
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
}
/* check for parameter updates */
@ -270,8 +261,6 @@ void Navigator::run()
// only update the reposition setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter,
// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)
bool reposition_valid = true;
vehicle_global_position_s position_setpoint{};
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
@ -285,11 +274,7 @@ void Navigator::run()
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
if (have_geofence_position_data) {
reposition_valid = geofence_allows_position(position_setpoint);
}
if (reposition_valid) {
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@ -433,18 +418,12 @@ void Navigator::run()
// only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl
bool change_altitude_valid = true;
vehicle_global_position_s position_setpoint{};
position_setpoint.lat = get_global_position()->lat;
position_setpoint.lon = get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
if (have_geofence_position_data) {
change_altitude_valid = geofence_allows_position(position_setpoint);
}
if (change_altitude_valid) {
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@ -514,18 +493,12 @@ void Navigator::run()
// for multicopters the orbit command is directly executed by the orbit flighttask
bool orbit_location_valid = true;
vehicle_global_position_s position_setpoint{};
position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat;
position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
if (have_geofence_position_data) {
orbit_location_valid = geofence_allows_position(position_setpoint);
}
if (orbit_location_valid) {
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.loiter_radius = get_loiter_radius();
@ -565,18 +538,12 @@ void Navigator::run()
#ifdef CONFIG_FIGURE_OF_EIGHT
// Only valid for fixed wing mode
bool orbit_location_valid = true;
vehicle_global_position_s position_setpoint{};
position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat;
position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
if (have_geofence_position_data) {
orbit_location_valid = geofence_allows_position(position_setpoint);
}
if (orbit_location_valid) {
if (geofence_allows_position(position_setpoint)) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.loiter_minor_radius = fabsf(get_loiter_radius());
@ -780,7 +747,7 @@ void Navigator::run()
check_traffic();
/* Check geofence violation */
geofence_breach_check(have_geofence_position_data);
geofence_breach_check();
/* Do stuff according to navigation state set by commander */
NavigatorMode *navigation_mode_new{nullptr};
@ -935,7 +902,7 @@ void Navigator::run()
}
}
void Navigator::geofence_breach_check(bool &have_geofence_position_data)
void Navigator::geofence_breach_check()
{
// reset the _time_loitering_after_gf_breach time if no longer in LOITER (and 100ms after it was triggered)
if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
@ -943,8 +910,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
_time_loitering_after_gf_breach = 0;
}
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
@ -986,6 +952,11 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
position_valid = _global_pos.timestamp > 0;
}
if (!position_valid) {
// we don't have a valid position yet, so we can't check for geofence violations
return;
}
_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
@ -1024,7 +995,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
}
_last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.geofence_action = _geofence.getGeofenceAction();