mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-07 05:20:34 +08:00
FlightTaskAuto: comments refactor
This commit is contained in:
Dennis Mannhart
committed by
Lorenz Meier
Lorenz Meier
parent
c01fab089a
commit
19743bcaec
@@ -74,21 +74,20 @@ bool FlightTaskAuto::updateInitialize()
|
||||
|
||||
bool FlightTaskAuto::_evaluateTriplets()
|
||||
{
|
||||
/* TODO: fix the issues mentioned below */
|
||||
/* We add here some conditions that are only required because
|
||||
* 1. navigator continuously sends triplet during mission due to yaw setpoint. This
|
||||
* should be removed in the navigator and only update once the current setpoint actually has changed.
|
||||
*
|
||||
* 2. navigator should be responsible to send always three valid setpoints. If there is only one setpoint,
|
||||
* then previous will be set to current vehicle position and next will be set equal to setpoint.
|
||||
*
|
||||
* 3. navigator originally only supports gps guided maneuvers. However, it now also supports some flow-specific features
|
||||
* such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the
|
||||
* takeoff/land was initiated. Until then we do this kind of logic here.
|
||||
*/
|
||||
// TODO: fix the issues mentioned below
|
||||
// We add here some conditions that are only required because:
|
||||
// 1. navigator continuously sends triplet during mission due to yaw setpoint. This
|
||||
// should be removed in the navigator and only updates if the current setpoint actually has changed.
|
||||
//
|
||||
// 2. navigator should be responsible to send always three valid setpoints. If there is only one setpoint,
|
||||
// then previous will be set to current vehicle position and next will be set equal to setpoint.
|
||||
//
|
||||
// 3. navigator originally only supports gps guided maneuvers. However, it now also supports some flow-specific features
|
||||
// such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the
|
||||
// takeoff/land was initiated. Until then we do this kind of logic here.
|
||||
|
||||
if (!_sub_triplet_setpoint->get().current.valid) {
|
||||
/* Best we can do is to just set all waypoints to current state */
|
||||
// best we can do is to just set all waypoints to current state
|
||||
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
|
||||
_yaw_wp = _yaw;
|
||||
_type = WaypointType::position;
|
||||
@@ -96,15 +95,15 @@ bool FlightTaskAuto::_evaluateTriplets()
|
||||
}
|
||||
|
||||
_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)) {
|
||||
/* Use default */
|
||||
// use default
|
||||
_mc_cruise_speed = _mc_cruise_default.get();
|
||||
}
|
||||
|
||||
/* Get target waypoint. */
|
||||
// get target waypoint.
|
||||
matrix::Vector3f target;
|
||||
map_projection_project(&_reference_position,
|
||||
_sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1));
|
||||
@@ -118,24 +117,22 @@ bool FlightTaskAuto::_evaluateTriplets()
|
||||
|
||||
}
|
||||
|
||||
/* 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.
|
||||
*/
|
||||
// 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.
|
||||
|
||||
/* Dont't do any updates if the current target has not changed */
|
||||
// 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 */
|
||||
// nothing has changed: just keep old waypoints
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Update all waypoints */
|
||||
// update all waypoints
|
||||
_target = target;
|
||||
|
||||
if (!PX4_ISFINITE(_target(0)) || !PX4_ISFINITE(_target(1))) {
|
||||
/* Horizontal target is not finite. */
|
||||
// Horizontal target is not finite. */
|
||||
_target(0) = _position(0);
|
||||
_target(1) = _position(1);
|
||||
}
|
||||
@@ -144,7 +141,7 @@ bool FlightTaskAuto::_evaluateTriplets()
|
||||
_target(2) = _position(2);
|
||||
}
|
||||
|
||||
_prev_prev_wp = _prev_wp; // previous -1 is set to previous
|
||||
_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,
|
||||
@@ -179,7 +176,7 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition()
|
||||
{
|
||||
FlightTask::_evaluateVehicleLocalPosition();
|
||||
|
||||
/* Check if reference has changed and update. */
|
||||
// check if reference has changed and update.
|
||||
if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference) {
|
||||
PX4_INFO("inside");
|
||||
map_projection_init(&_reference_position,
|
||||
|
||||
@@ -35,7 +35,6 @@
|
||||
* @file FlightTaskAuto.hpp
|
||||
*
|
||||
* Map from global triplet to local quadruple.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
@@ -45,9 +44,11 @@
|
||||
#include <uORB/topics/position_setpoint.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
/* This enum has to agree with position_setpoint_s type definition
|
||||
/**
|
||||
* This enum has to agree with position_setpoint_s type definition
|
||||
* The only reason for not using the struct position_setpoint is because
|
||||
* of the size */
|
||||
* of the size
|
||||
*/
|
||||
enum class WaypointType : int {
|
||||
position = 0,
|
||||
velocity,
|
||||
@@ -63,11 +64,8 @@ public:
|
||||
FlightTaskAuto(control::SuperBlock *parent, const char *name);
|
||||
|
||||
virtual ~FlightTaskAuto() = default;
|
||||
|
||||
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
|
||||
|
||||
bool activate() override;
|
||||
|
||||
bool updateInitialize() override;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -69,15 +69,15 @@ bool FlightTaskAutoLine::update()
|
||||
bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
|
||||
bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
|
||||
|
||||
/* 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state. */
|
||||
// 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
|
||||
if (follow_line && !follow_line_prev) {
|
||||
_reset();
|
||||
}
|
||||
|
||||
/* The only time a thrust setpoint is sent out is during
|
||||
* idle. Hence, reset thrust setpoint to NAN in case the
|
||||
* vehicle exits idle.
|
||||
*/
|
||||
// The only time a thrust set-point is sent out is during
|
||||
// idle. Hence, reset thrust set-point to NAN in case the
|
||||
// vehicle exits idle.
|
||||
|
||||
if (_type_previous == WaypointType::idle) {
|
||||
_thrust_setpoint = Vector3f(NAN, NAN, NAN);
|
||||
}
|
||||
@@ -98,13 +98,13 @@ bool FlightTaskAutoLine::update()
|
||||
_generateVelocitySetpoints();
|
||||
}
|
||||
|
||||
/* For now yaw setpoint comes directly form triplets.
|
||||
* TODO: In the future, however, yaw should be set in this
|
||||
* task based on flag: yaw along path, yaw based on gimbal, yaw
|
||||
* same as home yaw ... */
|
||||
// For now yaw-setpoint comes directly form triplets.
|
||||
// TODO: In the future, however, yaw should be set in this
|
||||
// task based on flag: yaw along path, yaw based on gimbal, yaw
|
||||
// same as home yaw ...
|
||||
_yaw_setpoint = _yaw_wp;
|
||||
|
||||
/* Update previous type */
|
||||
// update previous type
|
||||
_type_previous = _type;
|
||||
|
||||
return true;
|
||||
@@ -112,7 +112,7 @@ bool FlightTaskAutoLine::update()
|
||||
|
||||
void FlightTaskAutoLine::_reset()
|
||||
{
|
||||
/* Set setpoints equal current state. */
|
||||
// Set setpoints equal current state.
|
||||
_velocity_setpoint = _velocity;
|
||||
_position_setpoint = _position;
|
||||
_destination = _target;
|
||||
@@ -122,7 +122,7 @@ void FlightTaskAutoLine::_reset()
|
||||
|
||||
void FlightTaskAutoLine::_generateIdleSetpoints()
|
||||
{
|
||||
/* Send zero thrust setpoint */
|
||||
// Send zero thrust setpoint */
|
||||
_position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
|
||||
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
|
||||
_thrust_setpoint.zero();
|
||||
@@ -130,22 +130,22 @@ void FlightTaskAutoLine::_generateIdleSetpoints()
|
||||
|
||||
void FlightTaskAutoLine::_generateLandSetpoints()
|
||||
{
|
||||
/* Keep xy-position and go down with landspeed. */
|
||||
// Keep xy-position and go down with landspeed. */
|
||||
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
|
||||
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get()));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateTakeoffSetpoints()
|
||||
{
|
||||
/* Takeoff is completely defined by target position. */
|
||||
// Takeoff is completely defined by target position. */
|
||||
_position_setpoint = _target;
|
||||
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateVelocitySetpoints()
|
||||
{
|
||||
/* TODO: Remove velocity force logic from navigator, since
|
||||
* navigator should only send out waypoints. */
|
||||
// TODO: Remove velocity force logic from navigator, since
|
||||
// navigator should only send out waypoints.
|
||||
_position_setpoint = Vector3f(NAN, NAN, _position(2));
|
||||
Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
|
||||
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
|
||||
@@ -160,15 +160,14 @@ 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
|
||||
*/
|
||||
// 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. */
|
||||
// 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));
|
||||
@@ -176,7 +175,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
|
||||
if (u_prev_to_target * pos_to_target < 0.0f) {
|
||||
|
||||
/* Target is behind. */
|
||||
// Target is behind. */
|
||||
if (_current_state != State::target_behind) {
|
||||
|
||||
_destination = _target;
|
||||
@@ -186,8 +185,8 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
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 */
|
||||
// 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_rad.get())) {
|
||||
@@ -201,7 +200,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
|
||||
} 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. */
|
||||
// Current position is more than cruise speed in front of previous setpoint.
|
||||
if (_current_state != State::previous_infront) {
|
||||
_destination = _prev_wp;
|
||||
_origin = _position;
|
||||
@@ -210,8 +209,8 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
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 */
|
||||
// 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_rad.get())) {
|
||||
|
||||
@@ -225,7 +224,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
|
||||
} else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) {
|
||||
|
||||
/* Vehicle is more than cruise speed off track. */
|
||||
// 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;
|
||||
@@ -234,8 +233,8 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
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 */
|
||||
// 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_rad.get())) {
|
||||
|
||||
@@ -250,7 +249,7 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
} else {
|
||||
|
||||
if ((_target - _destination).length() > 0.01f) {
|
||||
/* A new target is available. Update speed at target.*/
|
||||
// A new target is available. Update speed at target.*/
|
||||
_destination = _target;
|
||||
_origin = _prev_wp;
|
||||
_current_state = State::none;
|
||||
@@ -258,8 +257,8 @@ void FlightTaskAutoLine::_updateInternalWaypoints()
|
||||
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 */
|
||||
// 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_rad.get())) {
|
||||
|
||||
@@ -281,14 +280,14 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) ||
|
||||
(!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) {
|
||||
|
||||
/* Vehicle reached target in xy and no passing required. Lock position */
|
||||
// Vehicle reached target in xy and no passing required. Lock position */
|
||||
_position_setpoint(0) = _destination(0);
|
||||
_position_setpoint(1) = _destination(1);
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
|
||||
|
||||
} else {
|
||||
|
||||
/* Get various path specific vectors. */
|
||||
// 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);
|
||||
@@ -297,45 +296,43 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
float speed_sp_track = _mc_cruise_speed;
|
||||
float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f);
|
||||
|
||||
/* Distance to target when brake should occur. The assumption is made that
|
||||
* 1.5 * cruising speed is enough to break. */
|
||||
// Distance to target when brake should occur. The assumption is made that
|
||||
// 1.5 * cruising speed is enough to break.
|
||||
float target_threshold = 1.5f * _mc_cruise_speed;
|
||||
float speed_threshold = _mc_cruise_speed;
|
||||
const float threshold_max = target_threshold;
|
||||
|
||||
if (target_threshold > 0.5f * prev_to_dest.length()) {
|
||||
/* Target threshold cannot be more than distance from previous to target */
|
||||
// Target threshold cannot be more than distance from previous to target
|
||||
target_threshold = 0.5f * prev_to_dest.length();
|
||||
}
|
||||
|
||||
/* Compute maximum speed at target threshold */
|
||||
// Compute maximum speed at target threshold */
|
||||
if (threshold_max > _nav_rad.get()) {
|
||||
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _nav_rad.get());
|
||||
speed_threshold = m * (target_threshold - _nav_rad.get()) + _speed_at_target; // speed at transition
|
||||
}
|
||||
|
||||
/* Either accelerate or decelerate */
|
||||
// Either accelerate or decelerate
|
||||
if (closest_to_dest.length() < target_threshold) {
|
||||
|
||||
/* Vehicle is close to destination. Start to decelerate */
|
||||
// Vehicle is close to destination. Start to decelerate
|
||||
|
||||
if (!has_reached_altitude) {
|
||||
/* Altitude is not reached yet. Vehicle has to stop first before proceeding */
|
||||
// Altitude is not reached yet. Vehicle has to stop first before proceeding
|
||||
_speed_at_target = 0.0f;
|
||||
}
|
||||
|
||||
float acceptance_radius = _nav_rad.get();
|
||||
|
||||
if (_speed_at_target < 0.01f) {
|
||||
/* If vehicle wants to stop at the target, then set acceptance radius
|
||||
* to zero as well.
|
||||
*/
|
||||
// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
|
||||
acceptance_radius = 0.0f;
|
||||
}
|
||||
|
||||
if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {
|
||||
|
||||
/* Slow down depending on distance to target minus acceptance radius */
|
||||
// Slow down depending on distance to target minus acceptance radius.
|
||||
float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius);
|
||||
speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition
|
||||
|
||||
@@ -343,10 +340,9 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
speed_sp_track = _speed_at_target;
|
||||
}
|
||||
|
||||
/* If we are close to target and the previous speed setpoint along track was smaller than
|
||||
* current speed setpoint along track, then take over the previous one.
|
||||
* This ensures smoothness since we anyway want to slow down.
|
||||
*/
|
||||
// If we are close to target and the previous speed setpoint along track was smaller than
|
||||
// current speed setpoint along track, then take over the previous one.
|
||||
// This ensures smoothness since we anyway want to slow down.
|
||||
if ((speed_sp_prev_track < speed_sp_track)
|
||||
&& (speed_sp_track * speed_sp_prev_track > 0.0f)
|
||||
&& (speed_sp_prev_track > _speed_at_target)) {
|
||||
@@ -355,7 +351,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
|
||||
} else {
|
||||
|
||||
/* Vehicle is still far from destination. Accelerate or keep maximum target speed. */
|
||||
// Vehicle is still far from destination. Accelerate or keep maximum target speed.
|
||||
float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
|
||||
|
||||
float yaw_diff = 0.0f;
|
||||
@@ -364,11 +360,11 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
yaw_diff = _wrap_pi(_yaw_wp - _yaw);
|
||||
}
|
||||
|
||||
/* If yaw offset is large, only accelerate with 0.5 m/s^2. */
|
||||
// If yaw offset is large, only accelerate with 0.5 m/s^2.
|
||||
float acc_max = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acc_xy.get();
|
||||
|
||||
if (acc_track > acc_max) {
|
||||
/* Accelerate towards target */
|
||||
// accelerate towards target
|
||||
speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
|
||||
}
|
||||
}
|
||||
@@ -385,50 +381,48 @@ void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
|
||||
void FlightTaskAutoLine::_generateAltitudeSetpoints()
|
||||
{
|
||||
/* Total distance between previous and target setpoint */
|
||||
// Total distance between previous and target set-point.
|
||||
const float dist = fabsf(_destination(2) - _origin(2));
|
||||
|
||||
/* If target has not been reached, then compute setpoint depending on maximum velocity */
|
||||
// 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)) {
|
||||
|
||||
/* get various distances */
|
||||
// get various distances */
|
||||
const float dist_to_prev = fabsf(_position(2) - _origin(2));
|
||||
const float dist_to_target = fabsf(_destination(2) - _position(2));
|
||||
|
||||
/* check sign */
|
||||
// check sign
|
||||
const bool flying_upward = _destination(2) < _position(2);
|
||||
|
||||
/* Speed at threshold is by default maximum speed. Threshold defines
|
||||
* the point in z at which vehicle slows down to reach target altitude. */
|
||||
// Speed at threshold is by default maximum speed. Threshold defines
|
||||
// the point in z at which vehicle slows down to reach target altitude.
|
||||
float speed_sp = (flying_upward) ? _vel_max_up.get() : _vel_max_down.get();
|
||||
|
||||
/* target threshold defines the distance to target(2) at which
|
||||
* the vehicle starts to slow down to approach the target smoothly */
|
||||
// Target threshold defines the distance to target(2) at which
|
||||
// the vehicle starts to slow down to approach the target smoothly.
|
||||
float target_threshold = speed_sp * 1.5f;
|
||||
|
||||
/* If the total distance in z is NOT 2x distance of target_threshold, we
|
||||
* will need to adjust the final_velocity in z */
|
||||
// If the total distance in z is NOT 2x distance of target_threshold, we
|
||||
// will need to adjust the final_velocity in z.
|
||||
const bool is_2_target_threshold = dist >= 2.0f * target_threshold;
|
||||
const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect
|
||||
const float slope = (speed_sp - min_vel) / (target_threshold); /* defines the the acceleration when slowing down */
|
||||
const float slope = (speed_sp - min_vel) / (target_threshold); // defines the the acceleration when slowing down */
|
||||
|
||||
if (!is_2_target_threshold) {
|
||||
/* adjust final_velocity since we are already are close
|
||||
* to target and therefore it is not necessary to accelerate
|
||||
* up to full speed
|
||||
*/
|
||||
// Adjust final_velocity since we are already are close to target and therefore it is not necessary to accelerate
|
||||
// upwards with full speed.
|
||||
target_threshold = dist * 0.5f;
|
||||
/* get the velocity at target_threshold */
|
||||
// get the velocity at target_threshold
|
||||
speed_sp = slope * (target_threshold) + min_vel;
|
||||
}
|
||||
|
||||
/* we want to slow down */
|
||||
// we want to slow down
|
||||
if (dist_to_target < target_threshold) {
|
||||
|
||||
speed_sp = slope * dist_to_target + min_vel;
|
||||
|
||||
} else if (dist_to_prev < target_threshold) {
|
||||
/* we want to accelerate */
|
||||
// we want to accelerate
|
||||
|
||||
const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
|
||||
const float acc_max = (flying_upward) ? (_acc_max_up.get() * 0.5f) : (_acc_max_down.get() * 0.5f);
|
||||
@@ -438,19 +432,19 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
|
||||
}
|
||||
}
|
||||
|
||||
/* make sure vel_sp_z is always positive */
|
||||
// make sure vel_sp_z is always positive
|
||||
if (speed_sp < 0.0f) {
|
||||
PX4_WARN("speed cannot be smaller than 0");
|
||||
speed_sp = 0.0f;
|
||||
}
|
||||
|
||||
/* get the sign of vel_sp_z */
|
||||
// get the sign of vel_sp_z
|
||||
_velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp;
|
||||
_position_setpoint(2) = NAN; // We don't care about position setpoint */
|
||||
_position_setpoint(2) = NAN; // We don't care about position setpoint
|
||||
|
||||
} else {
|
||||
|
||||
/* Vehicle reached desired target altitude */
|
||||
// vehicle reached desired target altitude
|
||||
_velocity_setpoint(2) = 0.0f;
|
||||
_position_setpoint(2) = _target(2);
|
||||
}
|
||||
@@ -458,16 +452,16 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
|
||||
|
||||
float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
|
||||
{
|
||||
/* Minimum cruise speed when passing waypoint */
|
||||
// minimum cruise speed when passing waypoint
|
||||
float min_cruise_speed = 0.0f;
|
||||
|
||||
/* Make sure that cruise speed is larger than minimum*/
|
||||
// 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. */
|
||||
// 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 = _cruise_speed_90.get();
|
||||
|
||||
if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
|
||||
@@ -478,34 +472,31 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
|
||||
middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f;
|
||||
}
|
||||
|
||||
/* If middle cruise speed is exactly in the middle, then compute
|
||||
* speed linearly
|
||||
*/
|
||||
// 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 */
|
||||
// compute speed sp at target
|
||||
float speed_close;
|
||||
|
||||
if (use_linear_approach) {
|
||||
|
||||
/* velocity close to target adjusted to angle
|
||||
* vel_close = m*x+q
|
||||
*/
|
||||
// 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 */
|
||||
// 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 */
|
||||
// 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;
|
||||
@@ -513,6 +504,6 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
|
||||
speed_close = a * powf(b, angle) + c;
|
||||
}
|
||||
|
||||
/* speed_close needs to be in between max and min */
|
||||
// speed_close needs to be in between max and min
|
||||
return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed);
|
||||
}
|
||||
|
||||
@@ -46,11 +46,8 @@ class FlightTaskAutoLine : public FlightTaskAuto
|
||||
{
|
||||
public:
|
||||
FlightTaskAutoLine(control::SuperBlock *parent, const char *name);
|
||||
|
||||
virtual ~FlightTaskAutoLine() = default;
|
||||
|
||||
bool activate() override;
|
||||
|
||||
bool update() override;
|
||||
|
||||
protected:
|
||||
|
||||
Reference in New Issue
Block a user