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

Merge pull request #3143 from PX4/takeoff_landing

Browse Source 
Takeoff landing
This commit is contained in:
Lorenz Meier
2015-11-18 12:42:36 +01:00
parent 090fef8ea3 3c1c2ba34e
commit 2e632cb84a
38 changed files with 2086 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+2 -4
View File
@@ -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
;
ROMFS/px4fmu_common/init.d/2101_fw_AERT
+5 -3
View File
@@ -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
ROMFS/px4fmu_common/init.d/2104_fw_AETR
+3 -1
View File
@@ -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
ROMFS/px4fmu_common/init.d/2105_maja
+50
View File
@@ -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 <andreas@uaventure.com>
#
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
ROMFS/px4fmu_common/init.d/2106_albatross
+51
View File
@@ -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 <andreas@uaventure.com>
#
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
ROMFS/px4fmu_common/mixers/AAERTWF.main.mix
+96
View File
@@ -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
ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix
+84
View File
@@ -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
cmake/configs/nuttx_px4fmu-v1_default.cmake
+2
View File
@@ -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
cmake/configs/nuttx_px4fmu-v2_default.cmake
+2
View File
@@ -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
cmake/configs/posix_eagle_default.cmake
+2
View File
@@ -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
cmake/configs/posix_sitl_simple.cmake
+2
View File
@@ -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
cmake/configs/qurt_eagle_hil.cmake
+2
View File
@@ -47,6 +47,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
modules/controllib
#
cmake/configs/qurt_eagle_release.cmake
+2
View File
@@ -70,6 +70,8 @@ set(config_module_list
lib/geo
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
#
# QuRT port
cmake/configs/qurt_eagle_travis.cmake
+2
View File
@@ -52,6 +52,8 @@ set(config_module_list
lib/geo_lookup
lib/conversion
lib/ecl
lib/terrain_estimation
lib/runway_takeoff
#
# QuRT port
msg/actuator_controls.msg
+3
View File
@@ -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
msg/navigation_capabilities.msg
+1
View File
@@ -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
msg/vehicle_attitude_setpoint.msg
+4
View File
@@ -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
src/lib/ecl/CMakeLists.txt
+1
View File
@@ -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
src/lib/ecl/attitude_fw/ecl_controller.h
+2
View File
@@ -75,6 +75,8 @@ struct ECL_ControlData {
float airspeed_max;
float airspeed;
float scaler;
float groundspeed;
float groundspeed_scaler;
bool lock_integrator;
};
src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
+153
View File
@@ -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 <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
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;
}
src/lib/ecl/attitude_fw/ecl_wheel_controller.h
+70
View File
@@ -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 <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*
* 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 <stdbool.h>
#include <stdint.h>
#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
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+5 -4
View File
@@ -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);
src/lib/geo/geo.c
+35
View File
@@ -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;
src/lib/geo/geo.h
+30
View File
@@ -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.
*
src/lib/runway_takeoff/CMakeLists.txt
+43
View File
@@ -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 :
src/lib/runway_takeoff/RunwayTakeoff.cpp
+286
View File
@@ -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 <roman@px4.io>
* @author Andreas Antener <andreas@uaventure.com>
*/
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "RunwayTakeoff.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <mathlib/mathlib.h>
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;
}
}
src/lib/runway_takeoff/RunwayTakeoff.h
+121
View File
@@ -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 <roman@px4.io>
* @author Andreas Antener <andreas@uaventure.com>
*/
#ifndef RUNWAYTAKEOFF_H
#define RUNWAYTAKEOFF_H
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <mavlink/mavlink_log.h>
#include <mathlib/mathlib.h>
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
src/lib/runway_takeoff/runway_takeoff_params.c
+137
View File
@@ -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 <andreas@uaventure.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/**
* 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);
src/lib/terrain_estimation/CMakeLists.txt
+42
View File
@@ -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 :
src/lib/terrain_estimation/terrain_estimator.cpp
+201
View File
@@ -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<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
matrix::Vector<float, 3> u;
u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity
} else {
_u_z = 0.0f;
}
// dynamics matrix
matrix::Matrix<float, n_x, n_x> A;
A.setZero();
A(0, 1) = 1;
A(1, 2) = 1;
// input matrix
matrix::Matrix<float, n_x, 1> B;
B.setZero();
B(1, 0) = 1;
// input noise variance
float R = 0.135f;
// process noise convariance
matrix::Matrix<float, n_x, n_x> Q;
Q(0, 0) = 0;
Q(1, 1) = 0;
// do prediction
matrix::Vector<float, n_x> 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<float, 1, n_x> C;
C(0, 0) = -1; // measured altitude,
float R = 0.009f;
matrix::Vector<float, 1> y;
y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch);
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0, 0) += R;
S_I = matrix::inv<float, 1> (S_I);
matrix::Vector<float, 1> r = y - C * _x;
matrix::Matrix<float, n_x, 1> 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<float, 1, n_x> C;
C(0, 1) = 1;
float R = 0.056f;
matrix::Vector<float, 1> y;
y(0) = gps->vel_d_m_s;
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0, 0) += R;
S_I = matrix::inv<float, 1>(S_I);
matrix::Vector<float, 1> r = y - C * _x;
matrix::Matrix<float, n_x, 1> 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;
}
}
src/lib/terrain_estimation/terrain_estimator.h
+100
View File
@@ -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 <lib/mathlib/mathlib.h>
#include "matrix/Matrix.hpp"
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/distance_sensor.h>
/*
* 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<float, n_x> _x; // state: ground distance, velocity, accel bias in z direction
float _u_z; // acceleration in earth z direction
matrix::Matrix<float, 3, 3> _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);
};
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+3
View File
@@ -69,6 +69,7 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <terrain_estimation/terrain_estimator.h>
#include <systemlib/perf_counter.h>
#include <lib/ecl/validation/data_validator_group.h>
#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;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+14 -4
View File
@@ -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;
src/modules/fw_att_control/fw_att_control_main.cpp
+131 -7
View File
@@ -82,6 +82,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <ecl/attitude_fw/ecl_wheel_controller.h>
#include <platforms/px4_defines.h>
/**
@@ -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 */
src/modules/fw_att_control/fw_att_control_params.c
+78
View File
@@ -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);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+300 -94
View File
File diff suppressed because it is too large Load Diff
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+12
View File
@@ -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);
src/modules/navigator/navigator_main.cpp
+9 -2
View File
@@ -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;