fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 07:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: move generation of the the internal waypoints to FlightTaskAuto such that they are available to all derived classes.

Browse Source 
Change logic to update the internal triplets only when the navigator triplets have changed or when the vehicle state has changed.
This commit is contained in:
Martina
2018-08-02 09:23:17 +02:00
committed by ChristophTobler
parent ade254394e
commit f379035a3b
6 changed files with 316 additions and 270 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+266 -58
View File
@@ -40,6 +40,9 @@
using namespace matrix;
#define SIGMA_SINGLE_OP 0.000001f
#define SIGMA_NORM 0.001f
bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!FlightTask::initializeSubscriptions(subscription_array)) {
@@ -60,7 +63,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
bool FlightTaskAuto::activate()
{
bool ret = FlightTask::activate();
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
_position_setpoint = _position;
_velocity_setpoint = _velocity;
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
_setDefaultConstraints();
return ret;
}
@@ -97,47 +103,96 @@ bool FlightTaskAuto::_evaluateTriplets()
// Check if triplet is valid. There must be at least a valid altitude.
if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) {
// best we can do is to just set all waypoints to current state and return false
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
// Best we can do is to just set all waypoints to current state and return false.
_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
_type = WaypointType::position;
return false;
}
_type = (WaypointType)_sub_triplet_setpoint->get().current.type;
// always update cruise speed since that can change without waypoint changes
// Always update cruise speed since that can change without waypoint changes.
_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _constraints.speed_xy)) {
// use default limit
// Use default limit.
_mc_cruise_speed = _constraints.speed_xy;
}
// get target waypoint.
matrix::Vector3f target;
// Temporary target variable where we save the local reprojection of the latest navigator current triplet.
matrix::Vector3f tmp_target;
if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat)
|| !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) {
// No position provided in xy. Lock position
if (!PX4_ISFINITE(_lock_position_xy(0))) {
target(0) = _lock_position_xy(0) = _position(0);
target(1) = _lock_position_xy(1) = _position(1);
_triplet_target(0) = _lock_position_xy(0) = _position(0);
_triplet_target(1) = _lock_position_xy(1) = _position(1);
} else {
target(0) = _lock_position_xy(0);
target(1) = _lock_position_xy(1);
_triplet_target(0) = _lock_position_xy(0);
_triplet_target(1) = _lock_position_xy(1);
_lock_position_xy *= NAN;
}
} else {
// Convert from global to local frame.
map_projection_project(&_reference_position,
_sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1));
_sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1));
}
target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
tmp_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
// check if target is valid
// Check if anything has changed. We do that by comparing the temporary target
// to the internal _triplet_target.
// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed.
bool triplet_update = true;
if (!(fabsf(_triplet_target(0) - tmp_target(0)) > 0.001f || fabsf(_triplet_target(1) - tmp_target(1)) > 0.001f
|| fabsf(_triplet_target(2) - tmp_target(2)) > 0.001f)) {
// Nothing has changed: just keep old waypoints.
triplet_update = false;
} else {
_triplet_target = tmp_target;
if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
// Horizontal target is not finite.
_triplet_target(0) = _position(0);
_triplet_target(1) = _position(1);
}
if (!PX4_ISFINITE(_triplet_target(2))) {
_triplet_target(2) = _position(2);
}
// If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp.
_prev_prev_wp = _triplet_prev_wp;
if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat,
_sub_triplet_setpoint->get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
_triplet_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude);
} else {
_triplet_prev_wp = _position;
}
if (_type == WaypointType::loiter) {
_triplet_next_wp = _triplet_target;
} else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat,
_sub_triplet_setpoint->get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
_triplet_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude);
} else {
_triplet_next_wp = _triplet_target;
}
}
// Check if yaw target is valid.
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) {
@@ -145,51 +200,13 @@ bool FlightTaskAuto::_evaluateTriplets()
_yaw_setpoint = NAN;
}
// Check if anything has changed. We do that by comparing the target
// setpoint to the previous target.
// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once tthey have changed.
// Calculate the current vehicle state and check if it has updated.
State _previous_state = _current_state;
_current_state = _getCurrentState();
bool state_update = (_current_state != _previous_state) ? true : false;
// dont't do any updates if the current target has not changed
if (!(fabsf(target(0) - _target(0)) > 0.001f || fabsf(target(1) - _target(1)) > 0.001f
|| fabsf(target(2) - _target(2)) > 0.001f)) {
// nothing has changed: just keep old waypoints
return true;
}
// update all waypoints
_target = target;
if (!PX4_ISFINITE(_target(0)) || !PX4_ISFINITE(_target(1))) {
// Horizontal target is not finite. */
_target(0) = _position(0);
_target(1) = _position(1);
}
if (!PX4_ISFINITE(_target(2))) {
_target(2) = _position(2);
}
_prev_prev_wp = _prev_wp;
if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat,
_sub_triplet_setpoint->get().previous.lon, &_prev_wp(0), &_prev_wp(1));
_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude);
} else {
_prev_wp = _position;
}
if (_type == WaypointType::loiter) {
_next_wp = _target;
} else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat,
_sub_triplet_setpoint->get().next.lon, &_next_wp(0), &_next_wp(1));
_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude);
} else {
_next_wp = _target;
if (triplet_update || state_update) {
_updateInternalWaypoints();
}
return true;
@@ -252,3 +269,194 @@ matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
return matrix::Vector2f{};
}
}
State FlightTaskAuto::_getCurrentState()
{
// Calculate the vehicle current state based on the Navigator triplets and the current position.
Vector2f u_prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0)).unit_or_zero();
Vector2f pos_to_target = Vector2f(&(_triplet_target - _position)(0));
Vector2f prev_to_pos = Vector2f(&(_position - _triplet_prev_wp)(0));
_closest_pt = Vector2f(&_triplet_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
State return_state = State::none;
if (u_prev_to_target * pos_to_target < 0.0f) {
// Target is behind.
return_state = State::target_behind;
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
// Current position is more than cruise speed in front of previous setpoint.
return_state = State::previous_infront;
} else if (Vector2f(Vector2f(&_position(0)) - _closest_pt).length() > _mc_cruise_speed) {
// Vehicle is more than cruise speed off track.
return_state = State::offtrack;
}
return return_state;
}
void FlightTaskAuto::_updateInternalWaypoints()
{
// The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp.
// The cases where it differs:
// 1. The vehicle already passed the target -> go straight to target
// 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
// 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
//
// If a new target is available, then the speed at the target is computed from the angle previous-target-next.
switch (_current_state) {
case State::target_behind: {
_target = _triplet_target;
_prev_wp = _position;
_next_wp = _triplet_next_wp;
//_current_state = State::target_behind;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
break;
case State::previous_infront: {
_next_wp = _triplet_target;
_target = _triplet_prev_wp;
_prev_wp = _position;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
break;
case State::offtrack: {
_next_wp = _triplet_target;
_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
_prev_wp = _position;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
break;
case State::none: {
_target = _triplet_target;
_prev_wp = _triplet_prev_wp;
_next_wp = _triplet_next_wp;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
angle =
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
break;
}
default:
break;
}
}
float FlightTaskAuto::_getVelocityFromAngle(const float angle)
{
// minimum cruise speed when passing waypoint
float min_cruise_speed = 0.0f;
// make sure that cruise speed is larger than minimum
if ((_mc_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
return _mc_cruise_speed;
}
// Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees.
// It needs to be always larger than minimum cruise speed.
float middle_cruise_speed = MPC_CRUISE_90.get();
if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
}
if ((_mc_cruise_speed - middle_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f;
}
// If middle cruise speed is exactly in the middle, then compute speed linearly.
bool use_linear_approach = false;
if (((_mc_cruise_speed + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) {
use_linear_approach = true;
}
// compute speed sp at target
float speed_close;
if (use_linear_approach) {
// velocity close to target adjusted to angle:
// vel_close = m*x+q
float slope = -(_mc_cruise_speed - min_cruise_speed) / 2.0f;
speed_close = slope * angle + _mc_cruise_speed;
} else {
// Speed close to target adjusted to angle x.
// speed_close = a *b ^x + c; where at angle x = 0 -> speed_close = cruise; angle x = 1 -> speed_close = middle_cruise_speed (this means that at 90degrees
// the velocity at target is middle_cruise_speed);
// angle x = 2 -> speed_close = min_cruising_speed
// from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c
float a = -((middle_cruise_speed - _mc_cruise_speed) * (middle_cruise_speed - _mc_cruise_speed))
/ (2.0f * middle_cruise_speed - _mc_cruise_speed - min_cruise_speed);
float c = _mc_cruise_speed - a;
float b = (middle_cruise_speed - c) / a;
speed_close = a * powf(b, angle) + c;
}
// speed_close needs to be in between max and min
return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
}
src/lib/FlightTasks/tasks/FlightTaskAuto.hpp
+27 -2
View File
@@ -61,6 +61,13 @@ enum class WaypointType : int {
follow_target
};
enum class State {
offtrack, /**< Vehicle is more than cruise speed away from track */
target_behind, /**< Vehicle is in front of target. */
previous_infront, /**< Vehilce is behind previous waypoint.*/
none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
};
class FlightTaskAuto : public FlightTask
{
public:
@@ -75,6 +82,7 @@ protected:
void _setDefaultConstraints() override;
float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
@@ -84,13 +92,28 @@ protected:
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
State _current_state{State::none};
float _speed_at_target = 0.0f; /**< Desired velocity at target. */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD // acceptance radius at which waypoints are updated move to line
); /**< Default mc cruise speed.*/
private:
matrix::Vector2f _lock_position_xy{NAN, NAN};
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE); /**< Default mc cruise speed.*/
matrix::Vector3f _triplet_target;
matrix::Vector3f _triplet_prev_wp;
matrix::Vector3f _triplet_next_wp;
matrix::Vector2f _closest_pt;
map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
float _reference_altitude = NAN; /**< Altitude relative to ground. */
@@ -99,4 +122,6 @@ private:
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();
};
src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+15 -185
View File
@@ -51,7 +51,6 @@ bool FlightTaskAutoLine::activate()
void FlightTaskAutoLine::_generateSetpoints()
{
_updateInternalWaypoints();
_generateAltitudeSetpoints();
_generateXYsetpoints();
@@ -62,141 +61,29 @@ void FlightTaskAutoLine::_generateSetpoints()
}
}
void FlightTaskAutoLine::_updateInternalWaypoints()
{
// The internal Waypoints might differ from previous_wp and target. The cases where it differs:
// 1. The vehicle already passed the target -> go straight to target
// 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
// 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
//
// If a new target is available, then the speed at the target is computed from the angle previous-target-next.
// Adjust destination and origin based on current vehicle state.
Vector2f u_prev_to_target = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
Vector2f pos_to_target = Vector2f(&(_target - _position)(0));
Vector2f prev_to_pos = Vector2f(&(_position - _prev_wp)(0));
Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target);
if (u_prev_to_target * pos_to_target < 0.0f) {
// Target is behind. */
if (_current_state != State::target_behind) {
_destination = _target;
_origin = _position;
_current_state = State::target_behind;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
// Current position is more than cruise speed in front of previous setpoint.
if (_current_state != State::previous_infront) {
_destination = _prev_wp;
_origin = _position;
_current_state = State::previous_infront;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
} else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) {
// Vehicle is more than cruise speed off track.
if (_current_state != State::offtrack) {
_destination = matrix::Vector3f(closest_pt(0), closest_pt(1), _target(2));
_origin = _position;
_current_state = State::offtrack;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
} else {
if ((_target - _destination).length() > 0.01f) {
// A new target is available. Update speed at target.*/
_destination = _target;
_origin = _prev_wp;
_current_state = State::none;
float angle = 2.0f;
_speed_at_target = 0.0f;
// angle = cos(x) + 1.0
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) {
angle =
Vector2f(&(_destination - _origin)(0)).unit_or_zero()
* Vector2f(&(_destination - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
}
}
void FlightTaskAutoLine::_generateXYsetpoints()
{
Vector2f pos_sp_to_dest = Vector2f(&(_destination - _position_setpoint)(0));
const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < NAV_ACC_RAD.get();
Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get();
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
(!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) {
// Vehicle reached target in xy and no passing required. Lock position */
_position_setpoint(0) = _destination(0);
_position_setpoint(1) = _destination(1);
_position_setpoint(0) = _target(0);
_position_setpoint(1) = _target(1);
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
} else {
// Get various path specific vectors. */
Vector2f u_prev_to_dest = Vector2f(&(_destination - _origin)(0)).unit_or_zero();
Vector2f prev_to_pos(&(_position - _origin)(0));
Vector2f closest_pt = Vector2f(&_origin(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f closest_to_dest = Vector2f(&(_destination - _position)(0));
Vector2f prev_to_dest = Vector2f(&(_destination - _origin)(0));
Vector2f u_prev_to_dest = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
Vector2f prev_to_pos(&(_position - _prev_wp)(0));
Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f closest_to_dest = Vector2f(&(_target - _position)(0));
Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
float speed_sp_track = _mc_cruise_speed;
float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f);
@@ -286,17 +173,17 @@ void FlightTaskAutoLine::_generateXYsetpoints()
void FlightTaskAutoLine::_generateAltitudeSetpoints()
{
// Total distance between previous and target set-point.
const float dist = fabsf(_destination(2) - _origin(2));
const float dist = fabsf(_target(2) - _prev_wp(2));
// If target has not been reached, then compute set-point depending on maximum velocity.
if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _destination(2)) > 0.1f)) {
if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) {
// get various distances */
const float dist_to_prev = fabsf(_position(2) - _origin(2));
const float dist_to_target = fabsf(_destination(2) - _position(2));
const float dist_to_prev = fabsf(_position(2) - _prev_wp(2));
const float dist_to_target = fabsf(_target(2) - _position(2));
// check sign
const bool flying_upward = _destination(2) < _position(2);
const bool flying_upward = _target(2) < _position(2);
// limit vertical downwards speed (positive z) close to ground
// for now we use the altitude above home and assume that we want to land at same height as we took off
@@ -356,67 +243,10 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// vehicle reached desired target altitude
_velocity_setpoint(2) = 0.0f;
_position_setpoint(2) = _destination(2);
_position_setpoint(2) = _target(2);
}
}
float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
{
// minimum cruise speed when passing waypoint
float min_cruise_speed = 0.0f;
// make sure that cruise speed is larger than minimum
if ((_mc_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
return _mc_cruise_speed;
}
// Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees.
// It needs to be always larger than minimum cruise speed.
float middle_cruise_speed = MPC_CRUISE_90.get();
if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
}
if ((_mc_cruise_speed - middle_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f;
}
// If middle cruise speed is exactly in the middle, then compute speed linearly.
bool use_linear_approach = false;
if (((_mc_cruise_speed + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) {
use_linear_approach = true;
}
// compute speed sp at target
float speed_close;
if (use_linear_approach) {
// velocity close to target adjusted to angle:
// vel_close = m*x+q
float slope = -(_mc_cruise_speed - min_cruise_speed) / 2.0f;
speed_close = slope * angle + _mc_cruise_speed;
} else {
// Speed close to target adjusted to angle x.
// speed_close = a *b ^x + c; where at angle x = 0 -> speed_close = cruise; angle x = 1 -> speed_close = middle_cruise_speed (this means that at 90degrees
// the velocity at target is middle_cruise_speed);
// angle x = 2 -> speed_close = min_cruising_speed
// from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c
float a = -((middle_cruise_speed - _mc_cruise_speed) * (middle_cruise_speed - _mc_cruise_speed))
/ (2.0f * middle_cruise_speed - _mc_cruise_speed - min_cruise_speed);
float c = _mc_cruise_speed - a;
float b = (middle_cruise_speed - c) / a;
speed_close = a * powf(b, angle) + c;
}
// speed_close needs to be in between max and min
return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
}
bool FlightTaskAutoLine::_highEnoughForLandingGear()
{
src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
+5 -5
View File
@@ -53,18 +53,18 @@ protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX,
(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR // acceleration in flight
)
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX
);
void _generateSetpoints() override;
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
private:
float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
};
src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp
+1 -5
View File
@@ -40,8 +40,7 @@
using namespace matrix;
#define SIGMA_SINGLE_OP 0.000001f
#define SIGMA_NORM 0.001f
bool FlightTaskAutoMapper::activate()
{
@@ -100,9 +99,6 @@ void FlightTaskAutoMapper::_reset()
// Set setpoints equal current state.
_velocity_setpoint = _velocity;
_position_setpoint = _position;
_yaw_setpoint = _yaw;
_destination = _position;
_origin = _position;
_speed_at_target = 0.0f;
}
src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp
+2 -15
View File
@@ -53,28 +53,15 @@ public:
protected:
matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */
matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */
float _speed_at_target = 0.0f; /**< Desired velocity at target. */
float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
enum class State {
offtrack, /**< Vehicle is more than cruise speed away from track */
target_behind, /**< Vehicle is in front of target. */
previous_infront, /**< Vehilce is behind previous waypoint.*/
none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
};
State _current_state{State::none};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_ACC_RAD>) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line
(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED
)
);
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */