From a5a204004b7251264befe3028440bebba9d83eaf Mon Sep 17 00:00:00 2001
From: Martina <martina@rivizzigno.it>
Date: Mon, 6 Aug 2018 16:27:42 +0200
Subject: [PATCH] FlightTaskAuto: add check for xy mission progress
---
.../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 33 +++++++++++++++++++
.../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 4 +++
2 files changed, 37 insertions(+)
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 40c9b55dc8..36062b97ac 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -221,6 +221,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_updateAvoidanceWaypoints();
}
+ _checkAvoidanceProgress();
+
return true;
}
@@ -304,6 +306,37 @@ void FlightTaskAuto::_updateAvoidanceWaypoints()
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
}
+void FlightTaskAuto::_checkAvoidanceProgress()
+{
+ position_controller_status_s pos_control_status = {};
+ pos_control_status.timestamp = hrt_absolute_time();
+
+ // vector from previous triplet to current target
+ Vector2f prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0));
+ // vector from previous triplet to the vehicle projected position on the line previous-target triplet
+ Vector2f prev_to_closest_pt = _closest_pt - Vector2f(&(_triplet_prev_wp)(0));
+ // fraction of the previous-tagerget line that has been flown
+ const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
+
+ if (prev_curr_travelled > 1.0f) {
+ // if the vehicle projected position on the line previous-target is past the target waypoint,
+ // increase the target acceptance radius such that navigator will update the triplets
+ pos_control_status.acceptance_radius = Vector2f(&(_triplet_target - _position)(0)).length() + 0.5f;
+ }
+
+ // do not check for waypoints yaw acceptance in navigator
+ pos_control_status.yaw_acceptance = NAN;
+
+ if (_pub_pos_control_status == nullptr) {
+ _pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
+
+ } else {
+ orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
+
+ }
+
+}
+
bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
{
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index c50e63e044..a80d34a563 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -43,6 +43,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/home_position.h>
+#include <uORB/topics/position_controller_status.h>
#include <lib/ecl/geo/geo.h>
/**
@@ -130,10 +131,13 @@ private:
WeatherVane *_ext_yaw_handler =
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+ orb_advert_t _pub_pos_control_status = nullptr; /**< Publisher for the position controller status */
+
bool _evaluateTriplets(); /**< Checks and sets triplets. */
bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
bool _evaluateGlobalReference(); /**< Check is global reference is available. */
float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */
State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
+ void _checkAvoidanceProgress();
};
--
GitLab