diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index b3b27874af7a..da0b01b97f24 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -2,9 +2,9 @@ uint64 timestamp # time since system start (microseconds) int8 LANDING_GEAR_UP = 1 # landing gear up int8 LANDING_GEAR_DOWN = -1 # landing gear down -float32 roll_body # body angle in NED frame -float32 pitch_body # body angle in NED frame -float32 yaw_body # body angle in NED frame +float32 roll_body # body angle in NED frame (can be NaN for FW) +float32 pitch_body # body angle in NED frame (can be NaN for FW) +float32 yaw_body # body angle in NED frame (can be NaN for FW) float32 yaw_sp_move_rate # rad/s (commanded by user) diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 62339338bacf..42e0c70f90d1 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -51,7 +51,6 @@ endif() # add core flight tasks to list list(APPEND flight_tasks_all - ManualStabilized ManualAltitude ManualAltitudeSmooth ManualPosition diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 759a0b70d3b3..9efe587ee47d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -150,7 +150,7 @@ class FlightTask : public ModuleParams * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy. * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method. */ - virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}; + virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } @@ -166,7 +166,7 @@ class FlightTask : public ModuleParams */ void _resetSetpoints(); - /* + /** * Check and update local position */ void _evaluateVehicleLocalPosition(); diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt index e3d9d8213295..14f9ce498624 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude FlightTaskManualAltitude.cpp ) -target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManualStabilized) +target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual) target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 0409451dd9c3..5cbc4636b781 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -43,19 +43,23 @@ using namespace matrix; bool FlightTaskManualAltitude::updateInitialize() { - bool ret = FlightTaskManualStabilized::updateInitialize(); - // in addition to stabilized require valid position and velocity in D-direction - return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)); + bool ret = FlightTaskManual::updateInitialize(); + // in addition to manual require valid position and velocity in D-direction and valid yaw + return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } bool FlightTaskManualAltitude::activate() { - bool ret = FlightTaskManualStabilized::activate(); - _thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity + bool ret = FlightTaskManual::activate(); + _yaw_setpoint = NAN; + _yawspeed_setpoint = 0.0f; + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.0f; _setDefaultConstraints(); + _constraints.tilt = math::radians(MPC_MAN_TILT_MAX.get()); + if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) { _constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; @@ -78,10 +82,9 @@ bool FlightTaskManualAltitude::activate() void FlightTaskManualAltitude::_scaleSticks() { - // reuse same scaling as for stabilized - FlightTaskManualStabilized::_scaleSticks(); + // Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed + _yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get()); - // scale horizontal velocity with expo curve stick input const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks_expo(2); } @@ -249,9 +252,41 @@ void FlightTaskManualAltitude::_respectMaxAltitude() } } +void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) +{ + float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; + Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); + v(0) = v_r(0); + v(1) = v_r(1); +} + +void FlightTaskManualAltitude::_updateHeadingSetpoints() +{ + /* Yaw-lock depends on stick input. If not locked, + * yaw_sp is set to NAN. + * TODO: add yawspeed to get threshold.*/ + if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { + // no fixed heading when rotating around yaw by stick + _yaw_setpoint = NAN; + + } else { + // hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + + } else { + // check reset counter and update yaw setpoint if necessary + if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) { + _yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); + _heading_reset_counter = _sub_attitude->get().quat_reset_counter; + } + } + } +} + void FlightTaskManualAltitude::_updateSetpoints() { - FlightTaskManualStabilized::_updateHeadingSetpoints(); // get yaw setpoint + _updateHeadingSetpoints(); // get yaw setpoint // Thrust in xy are extracted directly from stick inputs. A magnitude of // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no @@ -271,3 +306,11 @@ void FlightTaskManualAltitude::_updateSetpoints() _updateAltitudeLock(); } + +bool FlightTaskManualAltitude::update() +{ + _scaleSticks(); + _updateSetpoints(); + + return true; +} diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index f7bb7a51d07e..474c0cce6b9f 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -39,19 +39,26 @@ #pragma once -#include "FlightTaskManualStabilized.hpp" +#include "FlightTaskManual.hpp" -class FlightTaskManualAltitude : public FlightTaskManualStabilized +class FlightTaskManualAltitude : public FlightTaskManual { public: FlightTaskManualAltitude() = default; virtual ~FlightTaskManualAltitude() = default; bool activate() override; bool updateInitialize() override; + bool update() override; protected: - void _updateSetpoints() override; /**< updates all setpoints */ - void _scaleSticks() override; /**< scales sticks to velocity in z */ + void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ + virtual void _updateSetpoints(); /**< updates all setpoints */ + virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + + /** + * rotates vector into local frame + */ + void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /** * Check and sets for position lock. @@ -60,27 +67,15 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized */ void _updateAltitudeLock(); - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized, + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, (ParamFloat) MPC_HOLD_MAX_Z, (ParamInt) MPC_ALT_MODE, (ParamFloat) MPC_HOLD_MAX_XY, - (ParamFloat) MPC_Z_P + (ParamFloat) MPC_Z_P, /**< position controller altitude propotional gain */ + (ParamFloat) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */ + (ParamFloat) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */ ) private: - uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - float _max_speed_up = 10.0f; - float _min_speed_down = 1.0f; - bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ - - /** - * Distance to ground during terrain following. - * If user does not demand velocity change in D-direction and the vehcile - * is in terrain-following mode, then height to ground will be locked to - * _dist_to_ground_lock. - */ - float _dist_to_ground_lock = NAN; - /** * Terrain following. * During terrain following, the position setpoint is adjusted @@ -99,4 +94,19 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized void _respectMinAltitude(); void _respectMaxAltitude(); + + + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ + float _max_speed_up = 10.0f; + float _min_speed_down = 1.0f; + bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + + /** + * Distance to ground during terrain following. + * If user does not demand velocity change in D-direction and the vehcile + * is in terrain-following mode, then height to ground will be locked to + * _dist_to_ground_lock. + */ + float _dist_to_ground_lock = NAN; }; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 9e98b93fc0e7..12bc4586dea8 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -53,10 +53,11 @@ bool FlightTaskManualPosition::updateInitialize() bool FlightTaskManualPosition::activate() { - // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(); + _constraints.tilt = math::radians(MPC_TILTMAX_AIR.get()); + // set task specific constraint if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) { _constraints.speed_xy = MPC_VEL_MANUAL.get(); @@ -146,6 +147,13 @@ void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateSetpoints() { FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction + + // check if an external yaw handler is active and if yes, let it update the yaw setpoints + if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { + _yaw_setpoint = NAN; + _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); + } + _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints _updateXYlock(); // check for position lock } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 4b0b9ed46f51..04a2064fdfa1 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -51,6 +51,11 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude bool activate() override; bool updateInitialize() override; + /** + * Sets an external yaw handler which can be used to implement a different yaw control strategy. + */ + void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -65,4 +70,8 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude private: float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + + WeatherVane *_weathervane_yaw_handler = + nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + }; diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt deleted file mode 100644 index 1e12883dbeb9..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(FlightTaskManualStabilized - FlightTaskManualStabilized.cpp -) - -target_link_libraries(FlightTaskManualStabilized PUBLIC FlightTaskManual) -target_include_directories(FlightTaskManualStabilized PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp deleted file mode 100644 index 239823d8a4c6..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightManualStabilized.cpp - */ - -#include "FlightTaskManualStabilized.hpp" -#include -#include - -using namespace matrix; - -bool FlightTaskManualStabilized::activate() -{ - bool ret = FlightTaskManual::activate(); - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get()); - _yaw_setpoint = _yaw; - _yawspeed_setpoint = 0.0f; - _constraints.tilt = math::radians(_tilt_max_man.get()); - return ret; -} - -bool FlightTaskManualStabilized::updateInitialize() -{ - bool ret = FlightTaskManual::updateInitialize(); - // need a valid yaw-state - return ret && PX4_ISFINITE(_yaw); -} - -void FlightTaskManualStabilized::_scaleSticks() -{ - /* Scale sticks to yaw and thrust using - * linear scale for yaw and piecewise linear map for thrust. */ - _yawspeed_setpoint = _sticks_expo(3) * math::radians(_yaw_rate_scaling.get()); - _throttle = _throttleCurve(); -} - -void FlightTaskManualStabilized::_updateHeadingSetpoints() -{ - /* Yaw-lock depends on stick input. If not locked, - * yaw_sp is set to NAN. - * TODO: add yawspeed to get threshold.*/ - if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; - - } else { - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - - } else { - // check reset counter and update yaw setpoint if necessary - if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) { - _yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); - _heading_reset_counter = _sub_attitude->get().quat_reset_counter; - } - } - } - - // check if an external yaw handler is active and if yes, let it compute the yaw setpoints - if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { - _yaw_setpoint = NAN; - _yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate(); - } -} - -void FlightTaskManualStabilized::_updateThrustSetpoints() -{ - /* Rotate setpoint into local frame. */ - Vector2f sp(&_sticks(0)); - _rotateIntoHeadingFrame(sp); - - /* Ensure that maximum tilt is in [0.001, Pi] */ - float tilt_max = math::constrain(_constraints.tilt, 0.001f, M_PI_F); - - const float x = sp(0) * tilt_max; - const float y = sp(1) * tilt_max; - - /* The norm of the xy stick input provides the pointing - * direction of the horizontal desired thrust setpoint. The magnitude of the - * xy stick inputs represents the desired tilt. Both tilt and magnitude can - * be captured through Axis-Angle. - */ - /* The Axis-Angle is the perpendicular vector to xy-stick input */ - Vector2f v = Vector2f(y, -x); - float v_norm = v.norm(); // the norm of v defines the tilt angle - - if (v_norm > tilt_max) { // limit to the configured maximum tilt angle - v *= tilt_max / v_norm; - } - - /* The final thrust setpoint is found by rotating the scaled unit vector pointing - * upward by the Axis-Angle. - * Make sure that the attitude can be controlled even at 0 throttle. - */ - Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f); - _thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * math::max(_throttle, 0.0001f); -} - -void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v) -{ - float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; - Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); - v(0) = v_r(0); - v(1) = v_r(1); -} - -void FlightTaskManualStabilized::_updateSetpoints() -{ - _updateHeadingSetpoints(); - _updateThrustSetpoints(); -} - -float FlightTaskManualStabilized::_throttleCurve() -{ - // Scale stick z from [-1,1] to [min thrust, max thrust] - float throttle = -((_sticks(2) - 1.0f) * 0.5f); - - switch (_throttle_curve.get()) { - case 1: // no rescaling - return throttle; - - default: // 0 or other: rescale to hover throttle at 0.5 stick - if (throttle < 0.5f) { - return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get(); - - } else { - return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get(); - } - } - -} - -bool FlightTaskManualStabilized::update() -{ - _scaleSticks(); - _updateSetpoints(); - - return true; -} diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp deleted file mode 100644 index 60b9247f3462..000000000000 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightManualStabilized.hpp - * - * Flight task for manual controlled attitude. - * It generates thrust and yaw setpoints. - */ - -#pragma once - -#include "FlightTaskManual.hpp" - -class FlightTaskManualStabilized : public FlightTaskManual -{ -public: - FlightTaskManualStabilized() = default; - - virtual ~FlightTaskManualStabilized() = default; - bool activate() override; - bool updateInitialize() override; - bool update() override; - - /** - * Sets an external yaw handler which can be used to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;} - -protected: - virtual void _updateSetpoints(); /**< updates all setpoints */ - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ - virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */ - void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */ - -private: - void _updateThrustSetpoints(); /**< sets thrust setpoint */ - float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */ - - float _throttle{}; /** mapped from stick z */ - - WeatherVane *_ext_yaw_handler = - nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, - (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _tilt_max_man, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _throttle_min_stabilized, /**< minimum throttle for stabilized */ - (ParamFloat) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/ - (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ - (ParamInt) _throttle_curve /**< throttle curve behavior */ - ) -}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ae9b06caf7a2..eb95da8d183f 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -114,6 +114,20 @@ class MulticopterAttitudeControl : public ModuleBase void publish_rates_setpoint(); void publish_rate_controller_status(); + float throttle_curve(float throttle_stick_input); + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(float dt, bool reset_yaw_sp); + + /** + * Get the landing gear state based on the manual control switch position + * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN + */ + float get_landing_gear_state(); + + /** * Attitude controller. */ @@ -150,6 +164,7 @@ class MulticopterAttitudeControl : public ModuleBase orb_advert_t _v_rates_sp_pub{nullptr}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ + orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -186,6 +201,9 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + DEFINE_PARAMETERS( (ParamFloat) _roll_p, (ParamFloat) _roll_rate_p, @@ -221,6 +239,7 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _pitch_rate_max, (ParamFloat) _yaw_rate_max, (ParamFloat) _yaw_auto_max, + (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ (ParamFloat) _acro_roll_max, (ParamFloat) _acro_pitch_max, @@ -238,7 +257,14 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _board_offset_x, (ParamFloat) _board_offset_y, - (ParamFloat) _board_offset_z + (ParamFloat) _board_offset_z, + + /* Stabilized mode params */ + (ParamFloat) _man_tilt_max_deg, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _man_throttle_min, /**< minimum throttle for stabilized */ + (ParamFloat) _throttle_max, /**< maximum throttle for stabilized */ + (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ + (ParamInt) _throttle_curve /**< throttle curve behavior */ ) matrix::Vector3f _attitude_p; /**< P gain for attitude control */ @@ -251,6 +277,7 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _mc_rate_max; /**< attitude rate limits in stabilized modes */ matrix::Vector3f _auto_rate_max; /**< attitude rate limits in auto modes */ matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7c6a82047b75..c4858a5db364 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -177,6 +177,8 @@ MulticopterAttitudeControl::parameters_updated() _acro_rate_max(1) = math::radians(_acro_pitch_max.get()); _acro_rate_max(2) = math::radians(_acro_yaw_max.get()); + _man_tilt_max = math::radians(_man_tilt_max_deg.get()); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* get transformation matrix from sensor/board to body frame */ @@ -319,7 +321,15 @@ MulticopterAttitudeControl::vehicle_attitude_poll() orb_check(_v_att_sub, &updated); if (updated) { + uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); + + // Check for a heading reset + if (prev_quat_reset_counter != _v_att.quat_reset_counter) { + // we only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + } return true; } return false; @@ -368,6 +378,143 @@ MulticopterAttitudeControl::vehicle_land_detected_poll() } +float +MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) +{ + // throttle_stick_input is in range [0, 1] + switch (_throttle_curve.get()) { + case 1: // no rescaling to hover throttle + return _man_throttle_min.get() + throttle_stick_input * (_throttle_max.get() - _man_throttle_min.get()); + + default: // 0 or other: rescale to hover throttle at 0.5 stick + if (throttle_stick_input < 0.5f) { + return (_throttle_hover.get() - _man_throttle_min.get()) / 0.5f * throttle_stick_input + _man_throttle_min.get(); + + } else { + return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _throttle_max.get(); + } + } +} + +float +MulticopterAttitudeControl::get_landing_gear_state() +{ + // Only switch the landing gear up if we are not landed and if + // the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (_vehicle_land_detected.landed) { + _gear_state_initialized = false; + } + float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down + if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; + + } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + // Switching the gear off does put it into a safe defined state + _gear_state_initialized = true; + } + + return landing_gear; +} + +void +MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(Quatf(_v_att.q)).psi(); + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else if (_manual_control_sp.z > 0.05f) { + + const float yaw_rate = math::radians(_yaw_rate_scaling.get()); + attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate; + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(x*x + y*y) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + const float x = _manual_control_sp.x * _man_tilt_max; + const float y = _manual_control_sp.y * _man_tilt_max; + + // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane + Vector2f v = Vector2f(y, -x); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); + Eulerf euler_sp = q_sp_rpy; + attitude_setpoint.roll_body = euler_sp(0); + attitude_setpoint.pitch_body = euler_sp(1); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2); + + /* modify roll/pitch only if we're a VTOL */ + if (_vehicle_status.is_vtol) { + // Construct attitude setpoint rotation matrix. Modify the setpoints for roll + // and pitch such that they reflect the user's intention even if a large yaw error + // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix + // from the pure euler angle setpoints will lead to unexpected attitude behaviour from + // the user's view as the euler angle sequence uses the yaw setpoint and not the current + // heading of the vehicle. + // However there's also a coupling effect that causes oscillations for fast roll/pitch changes + // at higher tilt angles, so we want to avoid using this on multicopters. + // The effect of that can be seen with: + // - roll/pitch into one direction, keep it fixed (at high angle) + // - apply a fast yaw rotation + // - look at the roll and pitch angles: they should stay pretty much the same as when not yawing + + // calculate our current yaw error + float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw); + + // compute the vector obtained by rotating a z unit vector by the rotation + // given by the roll and pitch commands of the user + Vector3f zB = {0.0f, 0.0f, 1.0f}; + Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f); + Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB; + + // transform the vector into a new frame which is rotated around the z axis + // by the current yaw error. this vector defines the desired tilt when we look + // into the direction of the desired heading + Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); + z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; + + // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] + // R_tilt is computed from_euler; only true if cos(roll) not equal zero + // -> valid if roll is not +-pi/2; + attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1)); + attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); + } + + /* copy quaternion setpoint to attitude setpoint topic */ + Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); + q_sp.copyTo(attitude_setpoint.q_d); + attitude_setpoint.q_d_valid = true; + + attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z); + + attitude_setpoint.landing_gear = get_landing_gear_state(); + attitude_setpoint.timestamp = hrt_absolute_time(); + orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); +} + /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) @@ -586,14 +733,7 @@ MulticopterAttitudeControl::publish_rates_setpoint() _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); - - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - + orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); } void @@ -607,9 +747,7 @@ MulticopterAttitudeControl::publish_rate_controller_status() rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); - - int instance; - orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, nullptr, ORB_PRIO_DEFAULT); } void @@ -631,13 +769,7 @@ MulticopterAttitudeControl::publish_actuator_controls() } if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub != nullptr) { - - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - - } else if (_actuators_id) { - _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); - } + orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); } } @@ -681,6 +813,9 @@ MulticopterAttitudeControl::run() float dt_accumulator = 0.f; int loop_counter = 0; + bool reset_yaw_sp = true; + float attitude_dt = 0.f; + while (!should_exit()) { poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; @@ -738,19 +873,30 @@ MulticopterAttitudeControl::run() vehicle_land_detected_poll(); const bool manual_control_updated = vehicle_manual_poll(); const bool attitude_updated = vehicle_attitude_poll(); + attitude_dt += dt; /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't + * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { - if (fabsf(_manual_control_sp.y) > _rattitude_thres.get() || - fabsf(_manual_control_sp.x) > _rattitude_thres.get()) { - _v_control_mode.flag_control_attitude_enabled = false; - } + _v_control_mode.flag_control_attitude_enabled = + fabsf(_manual_control_sp.y) <= _rattitude_thres.get() && + fabsf(_manual_control_sp.x) <= _rattitude_thres.get(); } + bool attitude_setpoint_generated = false; + if (_v_control_mode.flag_control_attitude_enabled) { if (attitude_updated) { + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(attitude_dt, reset_yaw_sp); + attitude_setpoint_generated = true; + } + control_attitude(); publish_rates_setpoint(); } @@ -790,6 +936,13 @@ MulticopterAttitudeControl::run() } } + if (attitude_updated) { + reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || + _vehicle_land_detected.landed || + (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode + attitude_dt = 0.f; + } + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { dt_accumulator += dt; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 948562d82f40..0b50c5d7b4c2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -95,6 +95,9 @@ class MulticopterPositionControl : public ModuleBase /** @see ModuleBase::run() */ void run() override; + /** @see ModuleBase::print_status() */ + int print_status() override; + private: bool _in_smooth_takeoff = false; /**weathervane_enabled() && !(_control_mode.flag_control_manual_enabled - && !_control_mode.flag_control_position_enabled)) { + if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled + || _control_mode.flag_control_position_enabled)) { _wv_controller->activate(); } else { @@ -775,7 +789,7 @@ MulticopterPositionControl::run() local_pos_sp.vz = _control.getVelSp()(2); thr_sp.copyTo(local_pos_sp.thrust); - // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller). + // Publish local position setpoint (for logging only) publish_local_pos_sp(local_pos_sp); _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller @@ -831,18 +845,23 @@ void MulticopterPositionControl::start_flight_task() { bool task_failure = false; + bool should_disable_task = true; int prev_failure_count = _task_failure_count; + // Do not run any flight task for VTOLs in fixed-wing mode if (!_vehicle_status.is_rotary_wing) { _flight_tasks.switchTask(FlightTaskIndex::None); return; } if (_vehicle_status.in_transition_mode) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::Transition); if (error != 0) { - PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); + if (prev_failure_count == 0) { + PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error)); + } task_failure = true; _task_failure_count++; @@ -862,6 +881,7 @@ MulticopterPositionControl::start_flight_task() _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled)) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); if (error != 0) { @@ -879,6 +899,7 @@ MulticopterPositionControl::start_flight_task() // Auto-follow me if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); if (error != 0) { @@ -894,7 +915,8 @@ MulticopterPositionControl::start_flight_task() } } else if (_control_mode.flag_control_auto_enabled) { - // Auto relate maneuvers + // Auto related maneuvers + should_disable_task = false; int error = 0; switch (MPC_AUTO_MODE.get()) { case 0: @@ -928,6 +950,7 @@ MulticopterPositionControl::start_flight_task() // manual position control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { + should_disable_task = false; int error = 0; switch (MPC_POS_MODE.get()) { @@ -967,6 +990,7 @@ MulticopterPositionControl::start_flight_task() // manual altitude control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { + should_disable_task = false; int error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); if (error != 0) { @@ -982,24 +1006,6 @@ MulticopterPositionControl::start_flight_task() } } - // manual stabilized control - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL - || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { - int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized); - - if (error != 0) { - if (prev_failure_count == 0) { - PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error)); - } - task_failure = true; - _task_failure_count++; - - } else { - check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_STAB); - task_failure = false; - } - } - // check task failure if (task_failure) { @@ -1011,6 +1017,8 @@ MulticopterPositionControl::start_flight_task() // No task was activated. _flight_tasks.switchTask(FlightTaskIndex::None); } + } else if (should_disable_task) { + _flight_tasks.switchTask(FlightTaskIndex::None); } } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 7fdd1b55b29b..10cebefa9f3c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -334,7 +334,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * @unit deg * @min 0.0 - * @max 90.0 + * @max 180.0 * @decimal 1 * @group Multicopter Position Control */ @@ -568,7 +568,7 @@ PARAM_DEFINE_FLOAT(MPC_JERK_MIN, 8.0f); PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); /** - * Manual control stick exponential curve sensitivity attenuation with small velocity setpoints + * Manual position control stick exponential curve sensitivity * * The higher the value the less sensitivity the stick has around zero * while still reaching the maximum value with full stick deflection.