From 19743bcaec9ad86644f15e17897033de45d06736 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 29 Mar 2018 10:49:09 +0200 Subject: [PATCH] FlightTaskAuto: comments refactor --- src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 51 +++-- src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 10 +- .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 181 +++++++++--------- .../FlightTasks/tasks/FlightTaskAutoLine.hpp | 3 - 4 files changed, 114 insertions(+), 131 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 5eb4502375..3f3bd62058 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -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, diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index dd155571cc..a5924c998c 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -35,7 +35,6 @@ * @file FlightTaskAuto.hpp * * Map from global triplet to local quadruple. - * */ #pragma once @@ -45,9 +44,11 @@ #include #include -/* 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: diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 7049dce3b9..d272b6cbd9 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -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); } diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index a41e875f1d..5a983bfc0e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -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: