Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

FlightTaskAuto: move method getVelocityFromAngle to FlightTaskAutoLine

Browse Source
This commit is contained in:
Dennis Mannhart
2018-11-26 14:50:45 +01:00
committed by Daniel Agar
parent 23afb939f0
commit c97b2a3071
5 changed files with 46 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+19 -146
View File
@@ -462,102 +462,34 @@ void FlightTaskAuto::_updateInternalWaypoints()
// 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).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
case State::target_behind:
_target = _triplet_target;
_prev_wp = _position;
_next_wp = _triplet_next_wp;
break;
case State::previous_infront: {
_next_wp = _triplet_target;
_target = _triplet_prev_wp;
_target_acceptance_radius = _sub_triplet_setpoint->get().previous.acceptance_radius;
_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).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
case State::previous_infront:
_next_wp = _triplet_target;
_target = _triplet_prev_wp;
_prev_wp = _position;
break;
case State::offtrack: {
_next_wp = _triplet_target;
_target = 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).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle = Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).unit_or_zero()
+ 1.0f;
_speed_at_target = _getVelocityFromAngle(angle);
}
}
case State::offtrack:
_next_wp = _triplet_target;
_target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2));
_prev_wp = _position;
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).length() > 0.001f &&
(Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) {
angle =
Vector2f(_target - _prev_wp).unit_or_zero()
* Vector2f(_target - _next_wp).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;
break;
default:
break;
}
}
@@ -575,63 +507,4 @@ bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
// heading unknown and therefore do not change heading
return false;
}
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/Auto/FlightTaskAuto.hpp
-3
View File
@@ -102,8 +102,6 @@ protected:
uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
State _current_state{State::none};
float _speed_at_target = 0.0f; /**< Desired velocity at target. */
float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
@@ -140,7 +138,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(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
void _checkAvoidanceProgress();
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
+23 -1
View File
@@ -53,6 +53,24 @@ void FlightTaskAutoLine::_generateSetpoints()
_generateXYsetpoints();
}
void FlightTaskAutoLine::_setSpeedAtTarget()
{
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
float angle = 2.0f; // minimum angle
_speed_at_target = 0.0f;
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) {
// angle = cos(x) + 1.0
angle =
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = math::expontialFromLimits(angle, 0.0f, MPC_CRUISE_90.get(), _mc_cruise_speed);
}
}
void FlightTaskAutoLine::_generateHeadingAlongTrack()
{
Vector2f prev_to_dest(_target - _prev_wp);
@@ -62,18 +80,22 @@ void FlightTaskAutoLine::_generateHeadingAlongTrack()
void FlightTaskAutoLine::_generateXYsetpoints()
{
_setSpeedAtTarget();
Vector2f pos_sp_to_dest(_target - _position_setpoint);
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius;
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
(!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
// 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) = _target(0);
_position_setpoint(1) = _target(1);
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
} else {
// Vehicle needs to pass waypoint
// Ensure that vehicle never gets stuck because of too low target-speed
_speed_at_target = math::max(_speed_at_target, 0.5f);
// Get various path specific vectors. */
Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
+4
View File
@@ -62,4 +62,8 @@ protected:
void _generateHeadingAlongTrack(); /**< Generates heading along track. */
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:
void _setSpeedAtTarget(); /**< Sets desiered speed at target */
float _speed_at_target = 0.0f;
};
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
-1
View File
@@ -103,7 +103,6 @@ void FlightTaskAutoMapper::_reset()
// Set setpoints equal current state.
_velocity_setpoint = _velocity;
_position_setpoint = _position;
_speed_at_target = 0.0f;
}
void FlightTaskAutoMapper::_generateIdleSetpoints()