mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 21:03:34 +08:00
Small navigator refactor/cleanup
This commit is contained in:
Julian Kent
@@ -157,8 +157,6 @@ Navigator::run()
|
||||
/* rate-limit position subscription to 20 Hz / 50 ms */
|
||||
orb_set_interval(_local_pos_sub, 50);
|
||||
|
||||
hrt_abstime last_geofence_check = 0;
|
||||
|
||||
while (!should_exit()) {
|
||||
|
||||
/* wait for up to 1000ms for data */
|
||||
@@ -480,131 +478,7 @@ Navigator::run()
|
||||
check_traffic();
|
||||
|
||||
/* Check geofence violation */
|
||||
if (have_geofence_position_data &&
|
||||
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
|
||||
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
|
||||
|
||||
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
|
||||
|
||||
matrix::Vector2<double> fence_violation_test_point;
|
||||
geofence_violation_type_u gf_violation_type{};
|
||||
float test_point_bearing;
|
||||
float test_point_distance;
|
||||
float vertical_test_point_distance;
|
||||
|
||||
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
|
||||
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
|
||||
_gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs);
|
||||
_gf_breach_avoidance.setClimbRate(-_local_pos.vz);
|
||||
test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor();
|
||||
vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor();
|
||||
|
||||
} else {
|
||||
test_point_distance = 2.0f * get_loiter_radius();
|
||||
vertical_test_point_distance = 5.0f;
|
||||
|
||||
if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
|
||||
test_point_bearing = pos_ctrl_status.nav_bearing;
|
||||
|
||||
} else {
|
||||
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
|
||||
}
|
||||
}
|
||||
|
||||
_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
|
||||
_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
|
||||
_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
|
||||
_gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
|
||||
_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
|
||||
_gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
|
||||
|
||||
if (home_position_valid()) {
|
||||
_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
|
||||
}
|
||||
|
||||
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
|
||||
|
||||
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
|
||||
fence_violation_test_point(1),
|
||||
_global_pos.alt);
|
||||
|
||||
gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt +
|
||||
vertical_test_point_distance);
|
||||
|
||||
gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0),
|
||||
fence_violation_test_point(1),
|
||||
_global_pos.alt);
|
||||
|
||||
last_geofence_check = hrt_absolute_time();
|
||||
have_geofence_position_data = false;
|
||||
|
||||
_geofence_result.timestamp = hrt_absolute_time();
|
||||
_geofence_result.geofence_action = _geofence.getGeofenceAction();
|
||||
_geofence_result.home_required = _geofence.isHomeRequired();
|
||||
|
||||
if (gf_violation_type.value) {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = true;
|
||||
|
||||
/* Issue a warning about the geofence violation once */
|
||||
if (!_geofence_violation_warning_sent) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence");
|
||||
|
||||
// we have predicted a geofence violation and if the action is to loiter then
|
||||
// demand a reposition to a location which is inside the geofence
|
||||
if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) {
|
||||
position_setpoint_triplet_s *rep = get_reposition_triplet();
|
||||
|
||||
matrix::Vector2<double> lointer_center_lat_lon;
|
||||
matrix::Vector2<double> current_pos_lat_lon(_global_pos.lat, _global_pos.lon);
|
||||
float loiter_altitude_amsl = _global_pos.alt;
|
||||
|
||||
|
||||
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
// the computation of the braking distance does not match the actual braking distance. Until we have a better model
|
||||
// we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence
|
||||
lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type,
|
||||
&_geofence);
|
||||
|
||||
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type);
|
||||
|
||||
} else {
|
||||
|
||||
lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence);
|
||||
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type);
|
||||
}
|
||||
|
||||
rep->current.timestamp = hrt_absolute_time();
|
||||
rep->current.yaw = get_local_position()->heading;
|
||||
rep->current.yaw_valid = true;
|
||||
rep->current.lat = lointer_center_lat_lon(0);
|
||||
rep->current.lon = lointer_center_lat_lon(1);
|
||||
rep->current.alt = loiter_altitude_amsl;
|
||||
rep->current.valid = true;
|
||||
rep->current.loiter_radius = get_loiter_radius();
|
||||
rep->current.alt_valid = true;
|
||||
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
|
||||
rep->current.loiter_direction = 1;
|
||||
rep->current.cruising_throttle = get_cruising_throttle();
|
||||
rep->current.acceptance_radius = get_acceptance_radius();
|
||||
rep->current.cruising_speed = get_cruising_speed();
|
||||
|
||||
}
|
||||
|
||||
_geofence_violation_warning_sent = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = false;
|
||||
|
||||
/* Reset the _geofence_violation_warning_sent field */
|
||||
_geofence_violation_warning_sent = false;
|
||||
}
|
||||
|
||||
_geofence_result_pub.publish(_geofence_result);
|
||||
}
|
||||
geofence_breach_check(have_geofence_position_data);
|
||||
|
||||
/* Do stuff according to navigation state set by commander */
|
||||
NavigatorMode *navigation_mode_new{nullptr};
|
||||
@@ -822,6 +696,136 @@ Navigator::run()
|
||||
}
|
||||
}
|
||||
|
||||
void Navigator::geofence_breach_check(bool &have_geofence_position_data)
|
||||
{
|
||||
|
||||
if (have_geofence_position_data &&
|
||||
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
|
||||
(hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
|
||||
|
||||
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
|
||||
|
||||
matrix::Vector2<double> fence_violation_test_point;
|
||||
geofence_violation_type_u gf_violation_type{};
|
||||
float test_point_bearing;
|
||||
float test_point_distance;
|
||||
float vertical_test_point_distance;
|
||||
|
||||
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
|
||||
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
|
||||
_gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs);
|
||||
_gf_breach_avoidance.setClimbRate(-_local_pos.vz);
|
||||
test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor();
|
||||
vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor();
|
||||
|
||||
} else {
|
||||
test_point_distance = 2.0f * get_loiter_radius();
|
||||
vertical_test_point_distance = 5.0f;
|
||||
|
||||
if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
|
||||
test_point_bearing = pos_ctrl_status.nav_bearing;
|
||||
|
||||
} else {
|
||||
test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx);
|
||||
}
|
||||
}
|
||||
|
||||
_gf_breach_avoidance.setHorizontalTestPointDistance(test_point_distance);
|
||||
_gf_breach_avoidance.setVerticalTestPointDistance(vertical_test_point_distance);
|
||||
_gf_breach_avoidance.setTestPointBearing(test_point_bearing);
|
||||
_gf_breach_avoidance.setCurrentPosition(_global_pos.lat, _global_pos.lon, _global_pos.alt);
|
||||
_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
|
||||
_gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
|
||||
|
||||
if (home_position_valid()) {
|
||||
_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
|
||||
}
|
||||
|
||||
fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint();
|
||||
|
||||
gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),
|
||||
fence_violation_test_point(1),
|
||||
_global_pos.alt);
|
||||
|
||||
gf_violation_type.flags.max_altitude_exceeded = !_geofence.isBelowMaxAltitude(_global_pos.alt +
|
||||
vertical_test_point_distance);
|
||||
|
||||
gf_violation_type.flags.fence_violation = !_geofence.isInsidePolygonOrCircle(fence_violation_test_point(0),
|
||||
fence_violation_test_point(1),
|
||||
_global_pos.alt);
|
||||
|
||||
_last_geofence_check = hrt_absolute_time();
|
||||
have_geofence_position_data = false;
|
||||
|
||||
_geofence_result.timestamp = hrt_absolute_time();
|
||||
_geofence_result.geofence_action = _geofence.getGeofenceAction();
|
||||
_geofence_result.home_required = _geofence.isHomeRequired();
|
||||
|
||||
if (gf_violation_type.value) {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = true;
|
||||
|
||||
/* Issue a warning about the geofence violation once */
|
||||
if (!_geofence_violation_warning_sent) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence");
|
||||
|
||||
// we have predicted a geofence violation and if the action is to loiter then
|
||||
// demand a reposition to a location which is inside the geofence
|
||||
if (_geofence.getGeofenceAction() == geofence_result_s::GF_ACTION_LOITER) {
|
||||
position_setpoint_triplet_s *rep = get_reposition_triplet();
|
||||
|
||||
matrix::Vector2<double> lointer_center_lat_lon;
|
||||
matrix::Vector2<double> current_pos_lat_lon(_global_pos.lat, _global_pos.lon);
|
||||
float loiter_altitude_amsl = _global_pos.alt;
|
||||
|
||||
|
||||
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
// the computation of the braking distance does not match the actual braking distance. Until we have a better model
|
||||
// we set the loiter point to the current position, that will make sure that the vehicle will loiter inside the fence
|
||||
lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForMultirotor(gf_violation_type,
|
||||
&_geofence);
|
||||
|
||||
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForMulticopter(gf_violation_type);
|
||||
|
||||
} else {
|
||||
|
||||
lointer_center_lat_lon = _gf_breach_avoidance.generateLoiterPointForFixedWing(gf_violation_type, &_geofence);
|
||||
loiter_altitude_amsl = _gf_breach_avoidance.generateLoiterAltitudeForFixedWing(gf_violation_type);
|
||||
}
|
||||
|
||||
rep->current.timestamp = hrt_absolute_time();
|
||||
rep->current.yaw = get_local_position()->heading;
|
||||
rep->current.yaw_valid = true;
|
||||
rep->current.lat = lointer_center_lat_lon(0);
|
||||
rep->current.lon = lointer_center_lat_lon(1);
|
||||
rep->current.alt = loiter_altitude_amsl;
|
||||
rep->current.valid = true;
|
||||
rep->current.loiter_radius = get_loiter_radius();
|
||||
rep->current.alt_valid = true;
|
||||
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
|
||||
rep->current.loiter_direction = 1;
|
||||
rep->current.cruising_throttle = get_cruising_throttle();
|
||||
rep->current.acceptance_radius = get_acceptance_radius();
|
||||
rep->current.cruising_speed = get_cruising_speed();
|
||||
|
||||
}
|
||||
|
||||
_geofence_violation_warning_sent = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = false;
|
||||
|
||||
/* Reset the _geofence_violation_warning_sent field */
|
||||
_geofence_violation_warning_sent = false;
|
||||
}
|
||||
|
||||
_geofence_result_pub.publish(_geofence_result);
|
||||
}
|
||||
}
|
||||
|
||||
int Navigator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("navigator",
|
||||
|
||||
Reference in New Issue
Block a user