diff --git a/.travis.yml b/.travis.yml index e6b5239344..2aafa122e1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ matrix: - os: linux sudo: false - os: osx - osx_image: beta-xcode6.3 + osx_image: xcode7 sudo: true cache: @@ -60,11 +60,9 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update - && brew install astyle - && brew install gcc-arm-none-eabi + && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 3631a1e36c..d13514d03a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -6,8 +6,8 @@ # # @output MAIN1 aileron # @output MAIN2 elevator -# @output MAIN4 rudder -# @output MAIN3 throttle +# @output MAIN3 rudder +# @output MAIN4 throttle # @output MAIN5 flaps # # @output AUX1 feed-through of RC AUX1 channel @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 4 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 3113ede87a..f533ce66fe 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 3 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja new file mode 100644 index 0000000000..1f68428f3b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -0,0 +1,50 @@ +#!nsh +# +# @name Bormatec Maja +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 aileron +# @output MAIN3 elevator +# @output MAIN4 rudder +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAERTWF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross new file mode 100644 index 0000000000..79e560fb5f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -0,0 +1,51 @@ +#!nsh +# +# @name Applied Aeronautics Albatross +# +# @type Standard Plane +# +# @output MAIN1 aileron right +# @output MAIN2 aileron left +# @output MAIN3 v-tail right +# @output MAIN4 v-tail left +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps right +# @output MAIN8 flaps left +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAVVTWFF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix new file mode 100644 index 0000000000..2ab5bdb611 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix @@ -0,0 +1,96 @@ +Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. +The configuration assumes the aileron servo(s) are connected to PX4FMU servo +output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle +to output 4 and the wheel to output 5. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + + +Flaps / gimbal / payload mixer for last three channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix new file mode 100644 index 0000000000..e6520862d7 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -0,0 +1,84 @@ +Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps +using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU servo +output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4, the wheel to output 5 and the flaps to output 6 and 7. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Flaps mixer +------------ +Flap servos are physically reversed. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 0 5000 -10000 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 0 -5000 10000 -10000 10000 + diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ed48bd4f34..902447c282 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -111,6 +111,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0c6458cad1..c3a3586f66 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -118,6 +118,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index fc97e70918..76d9711187 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -38,6 +38,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..a7291a04f3 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -56,6 +56,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff ) set(config_extra_builtin_cmds diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 4222fff249..87dd29152e 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -47,6 +47,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 945e9da199..62a20263e5 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -70,6 +70,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index b9fda6ec48..75cb5f2394 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -52,6 +52,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/ecl + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 66e12325d2..a6ebda6aae 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg index 235b5df03b..6d12aaaed9 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/navigation_capabilities.msg @@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length +bool abort_landing diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31..43ea237b47 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,7 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 0fd5b5f5e9..9f8e02ba2e 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp l1/ecl_l1_pos_controller.cpp validation/data_validator.cpp validation/data_validator_group.cpp diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp new file mode 100644 index 0000000000..7e34ff2a61 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.cpp + * Implementation of a simple PID wheel controller for heading tracking. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_wheel_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") +{ +} + +ECL_WheelController::~ECL_WheelController() +{ +} + +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float min_speed = 1.0f; + + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { + + float id = _rate_error * dt * ctl_data.groundspeed_scaler; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling wheel"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h new file mode 100644 index 0000000000..f153a8f8f1 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_WheelController : + public ECL_Controller +{ +public: + ECL_WheelController(); + + ~ECL_WheelController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d8210549..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,15 @@ public: _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_CLOSEACC = 1 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a4b1599a9d..af49bf1504 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,6 +298,41 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) +{ + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + + } else if (dist >= FLT_EPSILON) { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + + } else { + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); + + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f77a8b58a4..11451fcf19 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -236,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); + +/** + * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing in rad + * @param distance in meters + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_target, double *lon_target); + /** * Returns the bearing to the next waypoint in radians. * diff --git a/src/lib/runway_takeoff/CMakeLists.txt b/src/lib/runway_takeoff/CMakeLists.txt new file mode 100644 index 0000000000..7938279a0e --- /dev/null +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 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_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + runway_takeoff_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..7089889858 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,286 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _throttle_ramp_time(2 * 1e6), + _start_wp(), + _runway_takeoff_enabled(this, "TKOFF"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); + _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) +{ + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > _nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { + _climbout = false; + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); + } + + break; + + default: + break; + } +} + +/* + * Returns true as long as we're below navigation altitude + */ +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; +} + +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_pitch_sp.get()); + } + + return tecsPitch; +} + +/* + * Returns the roll setpoint to use. + */ +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + // until we have enough ground clearance, set roll to 0 + if (_state < RunwayTakeoffState::CLIMBOUT) { + return 0.0f; + } + + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); + } + + return navigatorRoll; +} + +/* + * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. + */ +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } +} + +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? + throttle : + _takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; +} + +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_state < RunwayTakeoffState::FLY) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ +float RunwayTakeoff::getMaxPitch(float max) +{ + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); + } + + else { + return max; + } +} + +/* + * Returns the "previous" (start) WP for navigation. + */ +math::Vector<2> RunwayTakeoff::getStartWP() +{ + return _start_wp; +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..0e1c5ed14b --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + float getInitYaw() { return _init_yaw; }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); + math::Vector<2> getStartWP(); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..f5a1017f96 --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which we have enough ground clearance to allow some roll. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. + * + * @unit m + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during taxi / before takeoff airspeed is reached. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @unit deg + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @unit deg + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); diff --git a/src/lib/terrain_estimation/CMakeLists.txt b/src/lib/terrain_estimation/CMakeLists.txt new file mode 100644 index 0000000000..0c1177b96b --- /dev/null +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 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_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 0000000000..64a747f532 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + memset(&_x._data[0], 0, sizeof(_x._data)); + _u_z = 0.0f; + _P.setIdentity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) +{ + if (distance > 40.0f || distance < 0.00001f) { + return false; + + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + matrix::Matrix A; + A.setZero(); + A(0, 1) = 1; + A(1, 2) = 1; + + // input matrix + matrix::Matrix B; + B.setZero(); + B(1, 0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + matrix::Matrix Q; + Q(0, 0) = 0; + Q(1, 1) = 0; + + // do prediction + matrix::Vector dx = (A * _x) * dt; + dx(1) += B(1, 0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; +} + +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + matrix::Matrix C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + matrix::Vector y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + matrix::Matrix C; + C(0, 1) = 1; + + float R = 0.056f; + + matrix::Vector y; + y(0) = gps->vel_d_m_s; + + // residual + matrix::Matrix S_I = (C * _P * C.transpose()); + S_I(0, 0) += R; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; + + matrix::Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i, j))) { + reinit = true; + } + } + } + + if (reinit) { + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 0000000000..ed9e9741f3 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h + */ + +#include +#include "matrix/Matrix.hpp" +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information whether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator() {}; + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x = 3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + matrix::Matrix _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + /* + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + */ + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 1429979d6d..fc11e0f505 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -69,6 +69,7 @@ #include #include +#include #include #include #include "estimator_22states.h" @@ -278,6 +279,8 @@ private: AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _LP_att_P; math::LowPassFilter2p _LP_att_Q; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..49f08f3cab 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameters{}, _parameter_handles{}, _ekf(nullptr), + _terrain_estimator(nullptr), _LP_att_P(250.0f, 20.0f), _LP_att_Q(250.0f, 20.0f), @@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : { _voter_mag.set_timeout(200000); + _terrain_estimator = new TerrainEstimator(); + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); @@ -267,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } - + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; @@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main() // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); + // Publish attitude estimations publishAttitude(); @@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + } else { + _global_pos.terrain_alt_valid = false; + } _global_pos.yaw = _local_pos.yaw; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3ed5b3a5a3..8772ceb835 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include /** @@ -160,6 +161,10 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { float tconst; float p_p; @@ -181,6 +186,11 @@ private: float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -196,6 +206,9 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -222,6 +235,11 @@ private: param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -235,6 +253,9 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -248,6 +269,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -345,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ _ctrl_state = {}; @@ -380,6 +404,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -396,6 +426,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ @@ -458,6 +491,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -474,6 +513,9 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ @@ -502,6 +544,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); + return OK; } @@ -751,6 +800,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; @@ -778,9 +831,53 @@ FixedwingAttitudeControl::task_main() /* default flaps to center */ float flaps_control = 0.0f; + static float delta_flaps = 0; + /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps)) { - flaps_control = _manual.flaps; + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + + /* default flaperon to center */ + float flaperon = 0.0f; + + static float delta_flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; + } + + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; } /* decide if in stabilized or full manual control */ @@ -806,11 +903,19 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -824,6 +929,7 @@ FixedwingAttitudeControl::task_main() /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -835,6 +941,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { @@ -860,6 +967,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { @@ -879,6 +987,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else { /* @@ -929,6 +1038,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ @@ -952,17 +1062,23 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; + + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1002,13 +1118,21 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); @@ -1059,9 +1183,9 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } - _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 51c06fabb9..ce2db9ad5c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * @@ -374,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f21bfc5f2f..24cf1f084f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include @@ -95,6 +96,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -103,6 +105,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -194,9 +197,17 @@ private: bool land_onslope; bool land_useterrain; + // landing relevant states + float _t_alt_prev_valid; //**< last terrain estimate which was valid */ + hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ + hrt_abstime _time_started_landing; //*< time at which landing started */ + float height_flare; //*< estimated height to ground at which flare started */ + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -277,6 +288,7 @@ private: float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -325,6 +337,7 @@ private: param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -391,6 +404,11 @@ private: */ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + /** * Check if we are in a takeoff situation */ @@ -519,8 +537,13 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), + height_flare(0.0f), _was_in_air(false), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -562,6 +585,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -665,6 +689,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -715,6 +740,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -947,9 +974,9 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -958,6 +985,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + bool FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -1054,6 +1090,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1171,19 +1209,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (in_takeoff_situation()) { + float alt_sp; + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; + } + + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1196,7 +1254,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; + + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + //if (land_noreturn_vertical) { if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1207,7 +1278,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1236,12 +1307,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? @@ -1264,11 +1368,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1292,10 +1400,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1314,7 +1431,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1338,94 +1455,160 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); - last_sent = hrt_absolute_time(); + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); + + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; + + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } + // update runway takeoff helper + _runway_takeoff.update( + _ctrl_state.airspeed, + _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, + _mavlink_fd); - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // update tecs + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, - _parameters.throttle_cruise, - true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ - } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); - } } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); + last_sent = hrt_absolute_time(); + } - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } + } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); + } } } @@ -1620,9 +1803,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1637,15 +1827,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && @@ -1663,9 +1864,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } @@ -1825,6 +2030,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..14b73038a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e44b8d0be5..b056e3fd99 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,8 +440,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false;