mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-20 11:20:36 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Daniel Agar
|
9acd39521d |
@@ -92,7 +92,6 @@ pipeline {
|
||||
"px4_fmu-v2_fixedwing",
|
||||
"px4_fmu-v2_multicopter",
|
||||
"px4_fmu-v2_rover",
|
||||
"px4_fmu-v2_lto",
|
||||
"px4_fmu-v3_default",
|
||||
"px4_fmu-v4_default",
|
||||
"px4_fmu-v4pro_default",
|
||||
@@ -102,7 +101,6 @@ pipeline {
|
||||
"px4_fmu-v5_stackcheck",
|
||||
"px4_fmu-v5_uavcanv0periph",
|
||||
"px4_fmu-v5_uavcanv1",
|
||||
"px4_fmu-v5_lto",
|
||||
"px4_fmu-v5x_default",
|
||||
"px4_fmu-v6u_default",
|
||||
"px4_fmu-v6x_default",
|
||||
|
||||
+1
-9
@@ -99,7 +99,7 @@
|
||||
#
|
||||
#=============================================================================
|
||||
|
||||
cmake_minimum_required(VERSION 3.9 FATAL_ERROR)
|
||||
cmake_minimum_required(VERSION 3.2 FATAL_ERROR)
|
||||
|
||||
set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE)
|
||||
set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE)
|
||||
@@ -234,14 +234,6 @@ message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}")
|
||||
#
|
||||
project(px4 CXX C ASM)
|
||||
|
||||
# Check if LTO option and check if toolchain supports it
|
||||
if(LTO)
|
||||
include(CheckIPOSupported)
|
||||
check_ipo_supported()
|
||||
message(AUTHOR_WARNING "LTO enabled: LTO is highly experimental and should not be used in production")
|
||||
set(CMAKE_INTERPROCEDURAL_OPTIMIZATION TRUE)
|
||||
endif()
|
||||
|
||||
set(package-contact "px4users@googlegroups.com")
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
@@ -51,13 +51,6 @@ menu "Toolchain"
|
||||
string "Architecture"
|
||||
default ""
|
||||
|
||||
config BOARD_LTO
|
||||
bool "(EXPERIMENTAL) Link Time Optimization (LTO)"
|
||||
default n
|
||||
help
|
||||
Enables LTO flag in linker
|
||||
Note: Highly EXPERIMENTAL, furthermore make sure you're using a modern compiler GCC 9 or later
|
||||
|
||||
config BOARD_FULL_OPTIMIZATION
|
||||
bool "Full optmization (O3)"
|
||||
default n
|
||||
|
||||
@@ -25,8 +25,6 @@ param set-default FW_P_LIM_MIN -15
|
||||
param set-default FW_RR_FF 0.1
|
||||
param set-default FW_RR_P 0.3
|
||||
|
||||
param set-default FW_SPOILERS_LND 0.8
|
||||
|
||||
param set-default FW_THR_MAX 0.6
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_THR_CRUISE 0.25
|
||||
|
||||
@@ -21,16 +21,14 @@ O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
# mixer for the left aileron
|
||||
M: 2
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 -10000 -10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
# mixer for the right aileron
|
||||
M: 2
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
|
||||
# mixer for the elevator
|
||||
M: 1
|
||||
|
||||
@@ -8,22 +8,22 @@ R: 4x
|
||||
# tilt servo motor 1
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
# tilt servo motor 2
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
# tilt servo motor 3
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
# tilt servo motor 4
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
# mixer for the left aileron
|
||||
M: 1
|
||||
|
||||
@@ -10,7 +10,7 @@ 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).
|
||||
|
||||
Aileron mixer (roll + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -20,11 +20,11 @@ endpoints to suit.
|
||||
|
||||
M: 2
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 -10000 -10000 0 -10000 10000
|
||||
S: 0 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
Elevator mixer
|
||||
------------
|
||||
|
||||
@@ -11,7 +11,7 @@ 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 + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -21,11 +21,11 @@ endpoints to suit.
|
||||
|
||||
M: 2
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 -10000 -10000 0 -10000 10000
|
||||
S: 0 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
V-tail mixers
|
||||
-------------
|
||||
|
||||
@@ -9,20 +9,20 @@ 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), 3 (thrust) 4 (flaps), 5 (spoiler).
|
||||
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
|
||||
|
||||
Aileron mixer (roll + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up mechanically reversed.
|
||||
|
||||
M: 2
|
||||
S: 0 0 -10000 -10000 0 -10000 10000
|
||||
S: 0 5 10000 10000 0 -10000 10000
|
||||
S: 0 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
S: 0 0 -10000 -10000 0 -10000 10000
|
||||
S: 0 5 -10000 -10000 0 -10000 10000
|
||||
S: 0 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
V-tail mixers
|
||||
-------------
|
||||
|
||||
@@ -10,7 +10,7 @@ Tilt mechanism servo mixer
|
||||
M: 1
|
||||
|
||||
|
||||
S: 1 8 10000 10000 0 -10000 10000
|
||||
S: 1 4 10000 10000 0 -10000 10000
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
Tilt mechanism servo mixer
|
||||
---------------------------
|
||||
M: 1
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
Elevon mixers
|
||||
-------------
|
||||
|
||||
@@ -5,14 +5,14 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU
|
||||
|
||||
This file defines mixers suitable for controlling a fixed wing aircraft with
|
||||
aileron, v-tail (rudder, elevator) and throttle using PX4FMU.
|
||||
The configuration assumes the aileron servos are connected to PX4FMU
|
||||
The configuration assumes the aileron servos are connected to PX4FMU
|
||||
AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle
|
||||
to output 4.
|
||||
|
||||
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 + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -22,11 +22,11 @@ endpoints to suit.
|
||||
|
||||
M: 2
|
||||
S: 1 0 -10000 -10000 0 -10000 10000
|
||||
S: 1 5 10000 10000 0 -10000 10000
|
||||
S: 1 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
S: 1 0 -10000 -10000 0 -10000 10000
|
||||
S: 1 5 -10000 -10000 0 -10000 10000
|
||||
S: 1 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
|
||||
V-tail mixers
|
||||
|
||||
@@ -6,19 +6,19 @@
|
||||
---------------------------
|
||||
# front left up:2000 down:1000
|
||||
M: 1
|
||||
S: 1 8 0 -20000 10000 -10000 10000
|
||||
S: 1 4 0 -20000 10000 -10000 10000
|
||||
|
||||
# front right up:1000 down:2000
|
||||
M: 1
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
# rear left up:2000 down:1000
|
||||
M: 1
|
||||
S: 1 8 0 -20000 10000 -10000 10000
|
||||
S: 1 4 0 -20000 10000 -10000 10000
|
||||
|
||||
# rear right up:1000 down:2000
|
||||
M: 1
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
|
||||
|
||||
# Aileron mixer
|
||||
|
||||
@@ -10,12 +10,12 @@ Tilt mechanism servo mixer
|
||||
---------------------------
|
||||
#RIGHT up:2000 down:1000
|
||||
M: 2
|
||||
S: 1 8 0 -20000 10000 -10000 10000
|
||||
S: 1 4 0 -20000 10000 -10000 10000
|
||||
S: 0 2 8000 8000 0 -10000 10000
|
||||
|
||||
#LEFT up:1000 down:2000
|
||||
M: 2
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
S: 0 2 8000 8000 0 -10000 10000
|
||||
|
||||
Elevon mixers
|
||||
|
||||
@@ -10,7 +10,7 @@ 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).
|
||||
|
||||
Aileron mixer (roll + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -21,12 +21,12 @@ endpoints to suit.
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 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 5 -10000 -10000 0 -10000 10000
|
||||
S: 0 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
Elevator mixer
|
||||
------------
|
||||
|
||||
@@ -9,9 +9,9 @@ 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) 5 (spoiler).
|
||||
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
|
||||
|
||||
Aileron mixer (roll + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -22,12 +22,12 @@ endpoints to suit.
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 10000 10000 0 -10000 10000
|
||||
S: 0 5 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 5 -10000 -10000 0 -10000 10000
|
||||
S: 0 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
V-tail mixers
|
||||
-------------
|
||||
|
||||
@@ -10,13 +10,13 @@ Tilt mechanism servo mixer
|
||||
#RIGHT up:2000 down:1000
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 -20000 9000 -10000 10000
|
||||
S: 1 4 0 -20000 9000 -10000 10000
|
||||
S: 0 2 4000 4000 0 -10000 10000
|
||||
|
||||
#LEFT up:1000 down:2000
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
S: 0 2 4000 4000 0 -10000 10000
|
||||
|
||||
Elevon mixers
|
||||
|
||||
@@ -3,14 +3,14 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU
|
||||
|
||||
This file defines mixers suitable for controlling a fixed wing aircraft with
|
||||
aileron, v-tail (rudder, elevator) and throttle using PX4FMU.
|
||||
The configuration assumes the aileron servos are connected to PX4FMU
|
||||
The configuration assumes the aileron servos are connected to PX4FMU
|
||||
AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle
|
||||
to output 4.
|
||||
|
||||
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
|
||||
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 5 (spoiler).
|
||||
(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
|
||||
|
||||
Aileron mixer (roll + spoiler)
|
||||
Aileron mixer (roll + flaperon)
|
||||
---------------------------------
|
||||
|
||||
This mixer assumes that the aileron servos are set up correctly mechanically;
|
||||
@@ -21,12 +21,12 @@ endpoints to suit.
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 5 10000 10000 0 -10000 10000
|
||||
S: 1 6 10000 10000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 0 10000 10000 0 -10000 10000
|
||||
S: 1 5 -10000 -10000 0 -10000 10000
|
||||
S: 1 6 -10000 -10000 0 -10000 10000
|
||||
|
||||
|
||||
V-tail mixers
|
||||
|
||||
@@ -10,13 +10,13 @@ Tilt mechanism servo mixer
|
||||
#RIGHT up:2000 down:1000
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 -20000 10000 -10000 10000
|
||||
S: 1 4 0 -20000 10000 -10000 10000
|
||||
S: 0 2 8000 8000 0 -10000 10000
|
||||
|
||||
#LEFT up:1000 down:2000
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 1 8 0 20000 -10000 -10000 10000
|
||||
S: 1 4 0 20000 -10000 -10000 10000
|
||||
S: 0 2 8000 8000 0 -10000 10000
|
||||
|
||||
Elevon mixers
|
||||
|
||||
@@ -113,7 +113,7 @@
|
||||
#define RC_SERIAL_PORT "/dev/ttyS0"
|
||||
|
||||
// #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN10)
|
||||
#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true);
|
||||
|
||||
#define GPIO_FRSKY_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12)
|
||||
|
||||
@@ -149,7 +149,6 @@
|
||||
|
||||
/* RC Serial port */
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* power on/off */
|
||||
#define MS_PWR_BUTTON_DOWN 750
|
||||
|
||||
@@ -160,7 +160,6 @@
|
||||
#define HRT_PPM_CHANNEL /* T3C1 */ 1 /* use capture/compare channel 1 */
|
||||
#define GPIO_PPM_IN /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/**
|
||||
* GPIO PPM_IN on PB4 T3C1
|
||||
@@ -169,7 +168,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* RSSI_IN */
|
||||
|
||||
@@ -160,7 +160,6 @@
|
||||
#define HRT_PPM_CHANNEL /* T3C1 */ 1 /* use capture/compare channel 1 */
|
||||
#define GPIO_PPM_IN /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/**
|
||||
* GPIO PPM_IN on PB4 T3C1
|
||||
@@ -169,7 +168,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* RSSI_IN */
|
||||
|
||||
@@ -141,12 +141,6 @@
|
||||
* SPEKTRUM_RX (it's TX or RX in Bind) on PA10 UART1
|
||||
* The FMU can drive GPIO PPM_IN as an output
|
||||
*/
|
||||
// TODO?
|
||||
//#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6)
|
||||
//#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
//#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX)
|
||||
//#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
@@ -281,9 +281,7 @@
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS4"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
|
||||
#define PWMIN_TIMER 4
|
||||
@@ -327,7 +325,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -205,7 +205,6 @@
|
||||
|
||||
#define SPEKTRUM_RX_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(SPEKTRUM_RX_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(SPEKTRUM_RX_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -264,7 +264,6 @@
|
||||
|
||||
#define SPEKTRUM_RX_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(SPEKTRUM_RX_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(SPEKTRUM_RX_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -135,7 +135,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
|
||||
|
||||
@@ -158,7 +158,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -157,7 +157,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -134,7 +134,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
|
||||
|
||||
@@ -134,7 +134,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
|
||||
|
||||
@@ -134,7 +134,6 @@
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
|
||||
|
||||
@@ -312,7 +312,6 @@
|
||||
#define GPIO_PPM_IN /* GPIO_B1_06 GPT1_CAPTURE2 */ (GPIO_GPT1_CAPTURE2_2 | GENERAL_INPUT_IOMUX)
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* PWM input driver. Use FMU AUX5 pins attached to GPIO_EMC_33 GPIO3_IO19 FLEXPWM3_PWMA2 */
|
||||
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
CONFIG_BOARD_LTO=y
|
||||
@@ -128,7 +128,7 @@
|
||||
#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
|
||||
|
||||
/* For R12, this signal is active high. */
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
|
||||
#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
|
||||
|
||||
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
@@ -158,7 +158,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/**
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
CONFIG_BOARD_LTO=y
|
||||
@@ -274,7 +274,6 @@ static inline bool board_get_external_lockout_state(void)
|
||||
#define GPIO_nVDD_5V_HIPOWER_EN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12)
|
||||
#define GPIO_nVDD_5V_HIPOWER_OC /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13)
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_PASSIVE /* PE4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN4)
|
||||
#define GPIO_VDD_5V_RC_EN /* PG5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5)
|
||||
#define GPIO_VDD_5V_WIFI_EN /* PG6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5)
|
||||
#define GPIO_VDD_3V3_SD_CARD_EN /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7)
|
||||
@@ -314,9 +313,7 @@ static inline bool board_get_external_lockout_state(void)
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS4"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
|
||||
|
||||
/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
|
||||
@@ -349,7 +346,6 @@ static inline bool board_get_external_lockout_state(void)
|
||||
/* Power switch controls ******************************************************/
|
||||
|
||||
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
||||
#define SPEKTRUM_POWER_PASSIVE() px4_arch_configgpio(GPIO_VDD_3V3_SPEKTRUM_PASSIVE)
|
||||
#define SPEKTRUM_POWER_CONFIG() px4_arch_configgpio(GPIO_VDD_3V3_SPEKTRUM_POWER_EN)
|
||||
|
||||
/*
|
||||
@@ -363,7 +359,6 @@ static inline bool board_get_external_lockout_state(void)
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -212,10 +212,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
|
||||
VDD_5V_PERIPH_EN(true);
|
||||
VDD_5V_HIPOWER_EN(true);
|
||||
board_control_spi_sensors_power(true, 0xffff);
|
||||
#ifdef SPEKTRUM_POWER_PASSIVE
|
||||
// Turn power controls to passive
|
||||
SPEKTRUM_POWER_PASSIVE();
|
||||
#endif
|
||||
VDD_5V_RC_EN(true);
|
||||
VDD_5V_WIFI_EN(true);
|
||||
|
||||
|
||||
@@ -292,9 +292,7 @@
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* Input Capture Channels. */
|
||||
#define INPUT_CAP1_TIMER 5
|
||||
@@ -334,7 +332,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -258,10 +258,7 @@
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
#define RC_SERIAL_SWAP_RXTX
|
||||
|
||||
/* Input Capture Channels. */
|
||||
#define INPUT_CAP1_TIMER 1
|
||||
@@ -302,7 +299,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -316,9 +316,7 @@
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* Input Capture Channels. */
|
||||
#define INPUT_CAP1_TIMER 1
|
||||
@@ -359,7 +357,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -253,9 +253,7 @@
|
||||
#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2
|
||||
|
||||
/* RC Serial port */
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
|
||||
/* Input Capture Channels. */
|
||||
#define INPUT_CAP1_TIMER 5
|
||||
@@ -293,7 +291,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
#define SDIO_SLOTNO 0 /* Only one slot */
|
||||
|
||||
@@ -126,7 +126,7 @@
|
||||
#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
|
||||
|
||||
/* For,this signal is active high. */
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
|
||||
#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
|
||||
|
||||
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
@@ -157,7 +157,6 @@
|
||||
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
||||
/**
|
||||
|
||||
@@ -11,7 +11,6 @@ uint8 INDEX_AIRBRAKES = 6
|
||||
uint8 INDEX_LANDING_GEAR = 7
|
||||
uint8 INDEX_GIMBAL_SHUTTER = 3
|
||||
uint8 INDEX_CAMERA_ZOOM = 4
|
||||
uint8 INDEX_COLLECTIVE_TILT = 8
|
||||
|
||||
uint8 GROUP_INDEX_ATTITUDE = 0
|
||||
uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
|
||||
@@ -20,7 +19,7 @@ uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
|
||||
uint8 GROUP_INDEX_PAYLOAD = 6
|
||||
|
||||
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
|
||||
float32[9] control
|
||||
float32[8] control
|
||||
|
||||
# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
|
||||
# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
|
||||
|
||||
@@ -24,9 +24,4 @@ uint8 FLAPS_OFF = 0 # no flaps
|
||||
uint8 FLAPS_LAND = 1 # landing config flaps
|
||||
uint8 FLAPS_TAKEOFF = 2 # take-off config flaps
|
||||
|
||||
uint8 apply_spoilers # spoiler config specifier
|
||||
uint8 SPOILERS_OFF = 0 # no spoilers
|
||||
uint8 SPOILERS_LAND = 1 # landing config spoiler
|
||||
uint8 SPOILERS_DESCEND = 2 # descend config spoiler
|
||||
|
||||
# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
|
||||
|
||||
@@ -411,79 +411,6 @@ typedef uint8_t px4_guid_t[PX4_GUID_BYTE_LENGTH];
|
||||
************************************************************************************/
|
||||
__BEGIN_DECLS
|
||||
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_rc_singlewire
|
||||
*
|
||||
* Description:
|
||||
* A board may define RC_SERIAL_SINGLEWIRE, so that RC_SERIAL_PORT is configured
|
||||
* as singlewire UART.
|
||||
*
|
||||
* Input Parameters:
|
||||
* device: serial device, e.g. "/dev/ttyS0"
|
||||
*
|
||||
* Returned Value:
|
||||
* true singlewire should be enabled.
|
||||
* false if not.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#if defined(RC_SERIAL_SINGLEWIRE)
|
||||
static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
|
||||
#else
|
||||
static inline bool board_rc_singlewire(const char *device) { return false; }
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_rc_swap_rxtx
|
||||
*
|
||||
* Description:
|
||||
* A board may define RC_SERIAL_SWAP_RXTX, so that RC_SERIAL_PORT is configured
|
||||
* as UART with RX/TX swapped.
|
||||
*
|
||||
* Input Parameters:
|
||||
* device: serial device, e.g. "/dev/ttyS0"
|
||||
*
|
||||
* Returned Value:
|
||||
* true RX/RX should be swapped.
|
||||
* false if not.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#if defined(RC_SERIAL_SWAP_RXTX)
|
||||
static inline bool board_rc_swap_rxtx(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
|
||||
#else
|
||||
static inline bool board_rc_swap_rxtx(const char *device) { return false; }
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_rc_invert_input
|
||||
*
|
||||
* Description:
|
||||
* All boards may optionally define RC_INVERT_INPUT(bool invert) that is
|
||||
* used to invert the RC_SERIAL_PORT RC port (e.g. to toggle an external XOR via
|
||||
* GPIO).
|
||||
*
|
||||
* Input Parameters:
|
||||
* invert_on - A positive logic value, that when true (on) will set the HW in
|
||||
* inverted NRZ mode where a MARK will be 0 and SPACE will be a 1.
|
||||
*
|
||||
* Returned Value:
|
||||
* true the UART inversion got set.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#ifdef RC_INVERT_INPUT
|
||||
static inline bool board_rc_invert_input(const char *device, bool invert)
|
||||
{
|
||||
if (strcmp(device, RC_SERIAL_PORT) == 0) { RC_INVERT_INPUT(invert); return true; }
|
||||
|
||||
return false;
|
||||
}
|
||||
#else
|
||||
static inline bool board_rc_invert_input(const char *device, bool invert) { return false; }
|
||||
#endif
|
||||
|
||||
/* Provide an interface for reading the connected state of VBUS */
|
||||
|
||||
/************************************************************************************
|
||||
|
||||
@@ -47,11 +47,6 @@ DShot::DShot() :
|
||||
_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
|
||||
_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
|
||||
_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
|
||||
|
||||
if (_mixing_output.useDynamicMixing()) {
|
||||
// Avoid using the PWM failsafe params
|
||||
_mixing_output.setAllFailsafeValues(UINT16_MAX);
|
||||
}
|
||||
}
|
||||
|
||||
DShot::~DShot()
|
||||
|
||||
@@ -71,10 +71,6 @@ RCInput::RCInput(const char *device) :
|
||||
|
||||
RCInput::~RCInput()
|
||||
{
|
||||
#if defined(SPEKTRUM_POWER_PASSIVE)
|
||||
// Disable power controls for Spektrum receiver
|
||||
SPEKTRUM_POWER_PASSIVE();
|
||||
#endif
|
||||
dsm_deinit();
|
||||
|
||||
delete _crsf_telemetry;
|
||||
@@ -84,41 +80,6 @@ RCInput::~RCInput()
|
||||
perf_free(_publish_interval_perf);
|
||||
}
|
||||
|
||||
int
|
||||
RCInput::init()
|
||||
{
|
||||
#ifdef RF_RADIO_POWER_CONTROL
|
||||
// power radio on
|
||||
RF_RADIO_POWER_CONTROL(true);
|
||||
#endif // RF_RADIO_POWER_CONTROL
|
||||
|
||||
// dsm_init sets some file static variables and returns a file descriptor
|
||||
// it also powers on the radio if needed
|
||||
_rcs_fd = dsm_init(_device);
|
||||
|
||||
if (_rcs_fd < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (board_rc_swap_rxtx(_device)) {
|
||||
#if defined(TIOCSSWAP)
|
||||
ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
|
||||
#endif // TIOCSSWAP
|
||||
}
|
||||
|
||||
// assume SBUS input and immediately switch it to
|
||||
// so that if Single wire mode on TX there will be only
|
||||
// a short contention
|
||||
sbus_config(_rcs_fd, board_rc_singlewire(_device));
|
||||
|
||||
#ifdef GPIO_PPM_IN
|
||||
// disable CPPM input by mapping it away from the timer capture input
|
||||
px4_arch_unconfiggpio(GPIO_PPM_IN);
|
||||
#endif // GPIO_PPM_IN
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
RCInput::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
@@ -277,20 +238,28 @@ void RCInput::set_rc_scan_state(RC_SCAN newState)
|
||||
|
||||
void RCInput::rc_io_invert(bool invert)
|
||||
{
|
||||
// First check if the board provides a board-specific inversion method (e.g. via GPIO),
|
||||
// and if not use an IOCTL
|
||||
if (!board_rc_invert_input(_device, invert)) {
|
||||
#if defined(RC_SERIAL_PORT) && defined(RC_INVERT_INPUT)
|
||||
|
||||
if (strcmp(_device, RC_SERIAL_PORT) == 0) {
|
||||
RC_INVERT_INPUT(invert);
|
||||
}
|
||||
|
||||
#endif // RC_SERIAL_PORT && RC_INVERT_INPUT
|
||||
|
||||
|
||||
#if defined(TIOCSINVERT)
|
||||
|
||||
if (_rcs_fd >= 0) {
|
||||
if (invert) {
|
||||
//int ret_invert = ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
|
||||
ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX);
|
||||
|
||||
} else {
|
||||
ioctl(_rcs_fd, TIOCSINVERT, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#endif // TIOCSINVERT
|
||||
}
|
||||
}
|
||||
|
||||
void RCInput::Run()
|
||||
@@ -300,16 +269,7 @@ void RCInput::Run()
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_initialized) {
|
||||
if (init() == PX4_OK) {
|
||||
_initialized = true;
|
||||
|
||||
} else {
|
||||
PX4_ERR("init failed");
|
||||
exit_and_cleanup();
|
||||
}
|
||||
|
||||
} else {
|
||||
{
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
@@ -345,6 +305,17 @@ void RCInput::Run()
|
||||
|
||||
if (!_rc_scan_locked && !_armed) {
|
||||
if ((int)vcmd.param1 == 0) {
|
||||
|
||||
rc_io_invert(false);
|
||||
|
||||
if (_rcs_fd >= 0) {
|
||||
close(_rcs_fd);
|
||||
_rcs_fd = -1;
|
||||
}
|
||||
|
||||
// Configure serial port for DSM
|
||||
_rcs_fd = dsm_init(_device);
|
||||
|
||||
// DSM binding command
|
||||
int dsm_bind_mode = (int)vcmd.param2;
|
||||
|
||||
@@ -363,6 +334,8 @@ void RCInput::Run()
|
||||
bind_spektrum(dsm_bind_pulses);
|
||||
|
||||
cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
dsm_deinit();
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -418,7 +391,7 @@ void RCInput::Run()
|
||||
|
||||
// This block scans for a supported serial RC input and locks onto the first one found
|
||||
// Scan for 300 msec, then switch protocol
|
||||
constexpr hrt_abstime rc_scan_max = 300_ms;
|
||||
constexpr hrt_abstime rc_scan_max = 700_ms;
|
||||
|
||||
unsigned frame_drops = 0;
|
||||
|
||||
@@ -428,7 +401,8 @@ void RCInput::Run()
|
||||
// read all available data from the serial RC input UART
|
||||
|
||||
// read all available data from the serial RC input UART
|
||||
int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE);
|
||||
uint8_t rcs_buf[RC_MAX_BUFFER_SIZE];
|
||||
int newBytes = ::read(_rcs_fd, &rcs_buf[0], RC_MAX_BUFFER_SIZE);
|
||||
|
||||
if (newBytes > 0) {
|
||||
_bytes_rx += newBytes;
|
||||
@@ -441,14 +415,24 @@ void RCInput::Run()
|
||||
|
||||
case RC_SCAN_SBUS:
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
// Configure serial port for SBUS
|
||||
sbus_config(_rcs_fd, board_rc_singlewire(_device));
|
||||
|
||||
if (_rcs_fd < 0) {
|
||||
_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
|
||||
}
|
||||
|
||||
// assume SBUS input and immediately switch it to
|
||||
// so that if Single wire mode on TX there will be only
|
||||
// a short contention
|
||||
sbus_config(_rcs_fd, true);
|
||||
|
||||
|
||||
#if defined(TIOCSSINGLEWIRE)
|
||||
ioctl(_rcs_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
|
||||
#endif // TIOCSSINGLEWIRE
|
||||
|
||||
rc_io_invert(true);
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
_rc_scan_begin = hrt_absolute_time();
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
@@ -458,7 +442,7 @@ void RCInput::Run()
|
||||
bool sbus_failsafe = false;
|
||||
bool sbus_frame_drop = false;
|
||||
|
||||
rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
|
||||
rc_updated = sbus_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
|
||||
&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
@@ -471,8 +455,16 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
// Scan the next protocol
|
||||
rc_io_invert(false);
|
||||
|
||||
#if defined(TIOCSSINGLEWIRE)
|
||||
ioctl(_rcs_fd, TIOCSSINGLEWIRE, 0);
|
||||
#endif // TIOCSSINGLEWIRE
|
||||
|
||||
::close(_rcs_fd);
|
||||
_rcs_fd = -1;
|
||||
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_DSM);
|
||||
}
|
||||
|
||||
@@ -480,13 +472,20 @@ void RCInput::Run()
|
||||
|
||||
case RC_SCAN_DSM:
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
// Configure serial port for DSM
|
||||
dsm_config(_rcs_fd);
|
||||
// dsm_init sets some file static variables and returns a file descriptor
|
||||
// it also powers on the radio if needed
|
||||
if (_rcs_fd < 0) {
|
||||
//_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
|
||||
}
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
// Configure serial port for DSM
|
||||
_rcs_fd = dsm_init(_device);
|
||||
|
||||
if (_rcs_fd < 0) {
|
||||
PX4_ERR("dsm init failed");
|
||||
}
|
||||
|
||||
_rc_scan_begin = hrt_absolute_time();
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
@@ -496,7 +495,7 @@ void RCInput::Run()
|
||||
bool dsm_11_bit = false;
|
||||
|
||||
// parse new data
|
||||
rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
|
||||
rc_updated = dsm_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
|
||||
&dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
@@ -509,6 +508,9 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
dsm_deinit();
|
||||
_rcs_fd = -1;
|
||||
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_ST24);
|
||||
}
|
||||
@@ -519,11 +521,7 @@ void RCInput::Run()
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
// Configure serial port for DSM
|
||||
dsm_config(_rcs_fd);
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
_rcs_fd = dsm_init(_device);
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
@@ -537,7 +535,7 @@ void RCInput::Run()
|
||||
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
st24_rssi = input_rc_s::RSSI_MAX;
|
||||
rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
|
||||
rc_updated = (OK == st24_decode(rcs_buf[i], &st24_rssi, &lost_count,
|
||||
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
|
||||
}
|
||||
|
||||
@@ -560,6 +558,8 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
dsm_deinit();
|
||||
_rcs_fd = -1;
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_SUMD);
|
||||
}
|
||||
@@ -570,12 +570,9 @@ void RCInput::Run()
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
// Configure serial port for DSM
|
||||
_rcs_fd = dsm_init(_device);
|
||||
dsm_config(_rcs_fd);
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
|
||||
@@ -589,7 +586,7 @@ void RCInput::Run()
|
||||
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
sumd_rssi = input_rc_s::RSSI_MAX;
|
||||
rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
|
||||
rc_updated = (OK == sumd_decode(rcs_buf[i], &sumd_rssi, &rx_count,
|
||||
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
|
||||
}
|
||||
|
||||
@@ -603,6 +600,8 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
::close(_rcs_fd);
|
||||
_rcs_fd = -1;
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_PPM);
|
||||
}
|
||||
@@ -647,19 +646,20 @@ void RCInput::Run()
|
||||
case RC_SCAN_CRSF:
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
|
||||
if (_rcs_fd < 0) {
|
||||
_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
|
||||
}
|
||||
|
||||
// Configure serial port for CRSF
|
||||
crsf_config(_rcs_fd);
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
|
||||
// parse new data
|
||||
if (newBytes > 0) {
|
||||
rc_updated = crsf_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
|
||||
rc_updated = crsf_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,
|
||||
input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
@@ -686,6 +686,8 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
::close(_rcs_fd);
|
||||
_rcs_fd = -1;
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_GHST);
|
||||
}
|
||||
@@ -695,12 +697,13 @@ void RCInput::Run()
|
||||
case RC_SCAN_GHST:
|
||||
if (_rc_scan_begin == 0) {
|
||||
_rc_scan_begin = cycle_timestamp;
|
||||
// Configure serial port for GHST
|
||||
ghst_config(_rcs_fd);
|
||||
|
||||
// flush serial buffer and any existing buffered data
|
||||
tcflush(_rcs_fd, TCIOFLUSH);
|
||||
memset(_rcs_buf, 0, sizeof(_rcs_buf));
|
||||
// Configure serial port for GHST
|
||||
if (_rcs_fd < 0) {
|
||||
_rcs_fd = open(_device, O_RDWR | O_NONBLOCK);
|
||||
}
|
||||
|
||||
ghst_config(_rcs_fd);
|
||||
|
||||
} else if (_rc_scan_locked
|
||||
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
@@ -708,7 +711,7 @@ void RCInput::Run()
|
||||
// parse new data
|
||||
if (newBytes > 0) {
|
||||
int8_t ghst_rssi = -1;
|
||||
rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
|
||||
rc_updated = ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi,
|
||||
&_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS);
|
||||
|
||||
if (rc_updated) {
|
||||
@@ -736,6 +739,8 @@ void RCInput::Run()
|
||||
}
|
||||
|
||||
} else {
|
||||
::close(_rcs_fd);
|
||||
_rcs_fd = -1;
|
||||
// Scan the next protocol
|
||||
set_rc_scan_state(RC_SCAN_SBUS);
|
||||
}
|
||||
|
||||
@@ -129,7 +129,6 @@ private:
|
||||
|
||||
hrt_abstime _rc_scan_begin{0};
|
||||
|
||||
bool _initialized{false};
|
||||
bool _rc_scan_locked{false};
|
||||
bool _report_lock{true};
|
||||
|
||||
@@ -155,7 +154,6 @@ private:
|
||||
char _device[20] {}; ///< device / serial port path
|
||||
|
||||
static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE};
|
||||
uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {};
|
||||
|
||||
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
|
||||
uint16_t _raw_rc_count{};
|
||||
|
||||
+5
-7
@@ -189,8 +189,6 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
|
||||
static constexpr uint16_t offset = 903; // microseconds
|
||||
value = roundf(servo_position * 0.583f) + offset;
|
||||
|
||||
PX4_DEBUG(stderr, "CH%d=%d(0x%02x), ", channel, value, raw);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -470,6 +468,9 @@ int dsm_config(int fd)
|
||||
|
||||
void dsm_proto_init()
|
||||
{
|
||||
memset(&dsm_frame, 0, sizeof(dsm_frame_t));
|
||||
memset(&dsm_buf, 0, sizeof(dsm_buf_t));
|
||||
|
||||
dsm_channel_shift = 0;
|
||||
dsm_frame_drops = 0;
|
||||
dsm_chan_count = 0;
|
||||
@@ -509,11 +510,6 @@ int dsm_init(const char *device)
|
||||
|
||||
void dsm_deinit()
|
||||
{
|
||||
#ifdef SPEKTRUM_POWER_PASSIVE
|
||||
// Turn power controls to passive
|
||||
SPEKTRUM_POWER_PASSIVE();
|
||||
#endif
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
close(dsm_fd);
|
||||
}
|
||||
@@ -580,7 +576,9 @@ void dsm_bind(uint16_t cmd, int pulses)
|
||||
#if defined(DSM_DEBUG)
|
||||
printf("DSM: DSM_CMD_BIND_REINIT_UART\n");
|
||||
#endif
|
||||
#if defined(SPEKTRUM_RX_AS_UART)
|
||||
SPEKTRUM_RX_AS_UART();
|
||||
#endif // SPEKTRUM_RX_AS_UART
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
+4
-4
@@ -98,9 +98,9 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
|
||||
const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
_control_surfaces.applyFlapsAndAirbrakes(flaps_control, airbrakes_control, _first_control_surface_idx, actuator_sp);
|
||||
float control_flaps = -1.f; // For tilt-rotors INDEX_FLAPS is set as combined tilt. TODO: fix this
|
||||
float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
|
||||
_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -109,7 +109,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
|
||||
actuator_controls_s actuator_controls_1;
|
||||
|
||||
if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
|
||||
float control_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
|
||||
float control_tilt = actuator_controls_1.control[4] * 2.f - 1.f;
|
||||
|
||||
// set control_tilt to exactly -1 or 1 if close to these end points
|
||||
control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
|
||||
|
||||
@@ -44,5 +44,4 @@ px4_add_module(
|
||||
ecl_yaw_controller.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
SlewRate
|
||||
)
|
||||
|
||||
@@ -67,8 +67,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
|
||||
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
|
||||
|
||||
_rate_ctrl_status_pub.advertise();
|
||||
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
|
||||
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
|
||||
}
|
||||
|
||||
FixedwingAttitudeControl::~FixedwingAttitudeControl()
|
||||
@@ -384,14 +382,11 @@ void FixedwingAttitudeControl::Run()
|
||||
|
||||
/* if we are in rotary wing mode, do nothing */
|
||||
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
|
||||
_spoiler_setpoint_with_slewrate.setForcedValue(0.f);
|
||||
_flaps_setpoint_with_slewrate.setForcedValue(0.f);
|
||||
perf_end(_loop_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
controlFlaps(dt);
|
||||
controlSpoilers(dt);
|
||||
control_flaps(dt);
|
||||
|
||||
/* decide if in stabilized or full manual control */
|
||||
if (_vcontrol_mode.flag_control_rates_enabled) {
|
||||
@@ -503,11 +498,8 @@ void FixedwingAttitudeControl::Run()
|
||||
}
|
||||
|
||||
/* add trim increment if flaps are deployed */
|
||||
trim_roll += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_r_flps.get();
|
||||
trim_pitch += _flaps_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_flps.get();
|
||||
|
||||
// add trim increment from spoilers (only pitch)
|
||||
trim_pitch += _spoiler_setpoint_with_slewrate.getState() * _param_fw_dtrim_p_spoil.get();
|
||||
trim_roll += _flaps_applied * _param_fw_dtrim_r_flps.get();
|
||||
trim_pitch += _flaps_applied * _param_fw_dtrim_p_flps.get();
|
||||
|
||||
/* Run attitude controllers */
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
@@ -653,11 +645,11 @@ void FixedwingAttitudeControl::Run()
|
||||
_actuators.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
|
||||
* constrain(_actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
|
||||
|
||||
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
_actuators.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
|
||||
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
|
||||
_actuators.control[5] = _manual_control_setpoint.aux1;
|
||||
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
|
||||
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
|
||||
_actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
|
||||
_actuators.control[7] = _manual_control_setpoint.aux3;
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
@@ -705,16 +697,18 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tam
|
||||
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::controlFlaps(const float dt)
|
||||
void FixedwingAttitudeControl::control_flaps(const float dt)
|
||||
{
|
||||
/* default flaps to center */
|
||||
float flap_control = 0.0f;
|
||||
|
||||
/* map flaps by default to manual if valid */
|
||||
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
|
||||
flap_control = _manual_control_setpoint.flaps;
|
||||
if (PX4_ISFINITE(_manual_control_setpoint.flaps) && _vcontrol_mode.flag_control_manual_enabled
|
||||
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
|
||||
flap_control = _manual_control_setpoint.flaps * _param_fw_flaps_scl.get();
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled) {
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled
|
||||
&& fabsf(_param_fw_flaps_scl.get()) > 0.01f) {
|
||||
|
||||
switch (_att_sp.apply_flaps) {
|
||||
case vehicle_attitude_setpoint_s::FLAPS_OFF:
|
||||
@@ -722,54 +716,50 @@ void FixedwingAttitudeControl::controlFlaps(const float dt)
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::FLAPS_LAND:
|
||||
flap_control = _param_fw_flaps_lnd_scl.get();
|
||||
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_lnd_scl.get();
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
|
||||
flap_control = _param_fw_flaps_to_scl.get();
|
||||
flap_control = 1.0f * _param_fw_flaps_scl.get() * _param_fw_flaps_to_scl.get();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// move the actual control value continuous with time, full flap travel in 1sec
|
||||
_flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt);
|
||||
}
|
||||
if (fabsf(_flaps_applied - flap_control) > 0.01f) {
|
||||
_flaps_applied += (_flaps_applied - flap_control) < 0 ? dt : -dt;
|
||||
|
||||
void FixedwingAttitudeControl::controlSpoilers(const float dt)
|
||||
{
|
||||
float spoiler_control = 0.f;
|
||||
} else {
|
||||
_flaps_applied = flap_control;
|
||||
}
|
||||
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
switch (_param_fw_spoilers_man.get()) {
|
||||
case 0:
|
||||
break;
|
||||
/* default flaperon to center */
|
||||
float flaperon_control = 0.0f;
|
||||
|
||||
case 1:
|
||||
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
|
||||
break;
|
||||
/* map flaperons by default to manual if valid */
|
||||
if (PX4_ISFINITE(_manual_control_setpoint.aux2) && _vcontrol_mode.flag_control_manual_enabled
|
||||
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
|
||||
|
||||
case 2:
|
||||
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? _manual_control_setpoint.aux1 : 0.f;
|
||||
break;
|
||||
}
|
||||
flaperon_control = 0.5f * (_manual_control_setpoint.aux2 + 1.0f) * _param_fw_flaperon_scl.get();
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled) {
|
||||
switch (_att_sp.apply_spoilers) {
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
|
||||
spoiler_control = 0.f;
|
||||
break;
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled
|
||||
&& fabsf(_param_fw_flaperon_scl.get()) > 0.01f) {
|
||||
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
|
||||
spoiler_control = _param_fw_spoilers_lnd.get();
|
||||
break;
|
||||
if (_att_sp.apply_flaps == vehicle_attitude_setpoint_s::FLAPS_LAND) {
|
||||
flaperon_control = _param_fw_flaperon_scl.get();
|
||||
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
|
||||
spoiler_control = _param_fw_spoilers_desc.get();
|
||||
break;
|
||||
} else {
|
||||
flaperon_control = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt);
|
||||
// move the actual control value continuous with time, full flap travel in 1sec
|
||||
if (fabsf(_flaperons_applied - flaperon_control) > 0.01f) {
|
||||
_flaperons_applied += (_flaperons_applied - flaperon_control) < 0 ? dt : -dt;
|
||||
|
||||
} else {
|
||||
_flaperons_applied = flaperon_control;
|
||||
}
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
|
||||
|
||||
@@ -40,7 +40,6 @@
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
@@ -79,9 +78,6 @@ using uORB::SubscriptionData;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s]
|
||||
static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
|
||||
|
||||
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
{
|
||||
@@ -143,6 +139,9 @@ private:
|
||||
|
||||
hrt_abstime _last_run{0};
|
||||
|
||||
float _flaps_applied{0.0f};
|
||||
float _flaperons_applied{0.0f};
|
||||
|
||||
float _airspeed_scaling{1.0f};
|
||||
|
||||
bool _landed{true};
|
||||
@@ -157,9 +156,6 @@ private:
|
||||
float _control_energy[4] {};
|
||||
float _control_prev[3] {};
|
||||
|
||||
SlewRate<float> _spoiler_setpoint_with_slewrate;
|
||||
SlewRate<float> _flaps_setpoint_with_slewrate;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
|
||||
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
|
||||
@@ -175,7 +171,6 @@ private:
|
||||
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
|
||||
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
|
||||
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
|
||||
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
|
||||
@@ -184,11 +179,10 @@ private:
|
||||
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
|
||||
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
|
||||
|
||||
(ParamFloat<px4::params::FW_FLAPERON_SCL>) _param_fw_flaperon_scl,
|
||||
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
|
||||
(ParamFloat<px4::params::FW_FLAPS_SCL>) _param_fw_flaps_scl,
|
||||
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
|
||||
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
|
||||
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
|
||||
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
|
||||
|
||||
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
|
||||
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
|
||||
@@ -236,19 +230,7 @@ private:
|
||||
ECL_YawController _yaw_ctrl;
|
||||
ECL_WheelController _wheel_ctrl;
|
||||
|
||||
/**
|
||||
* @brief Update flap control setting
|
||||
*
|
||||
* @param dt Current time delta [s]
|
||||
*/
|
||||
void controlFlaps(const float dt);
|
||||
|
||||
/**
|
||||
* @brief Update spoiler control setting
|
||||
*
|
||||
* @param dt Current time delta [s]
|
||||
*/
|
||||
void controlSpoilers(const float dt);
|
||||
void control_flaps(const float dt);
|
||||
|
||||
void updateActuatorControlsStatus(float dt);
|
||||
|
||||
|
||||
@@ -457,11 +457,22 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
|
||||
|
||||
/**
|
||||
* Scale factor for flaps
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Flaps setting during take-off
|
||||
*
|
||||
* Sets a fraction of full flaps during take-off.
|
||||
* Also applies to flaperons if enabled in the mixer/allocation.
|
||||
* Sets a fraction of full flaps (FW_FLAPS_SCL) during take-off
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
@@ -475,8 +486,7 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
|
||||
/**
|
||||
* Flaps setting during landing
|
||||
*
|
||||
* Sets a fraction of full flaps during landing.
|
||||
* Also applies to flaperons if enabled in the mixer/allocation.
|
||||
* Sets a fraction of full flaps (FW_FLAPS_SCL) during landing
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
@@ -487,6 +497,18 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
|
||||
|
||||
/**
|
||||
* Scale factor for flaperons
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);
|
||||
|
||||
/**
|
||||
* Airspeed mode
|
||||
*
|
||||
@@ -713,52 +735,3 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);
|
||||
|
||||
/**
|
||||
* Pitch trim increment for spoiler configuration
|
||||
*
|
||||
* This increment is added to the pitch trim whenever spoilers are fully deployed.
|
||||
*
|
||||
* @group FW Attitude Control
|
||||
* @min -0.25
|
||||
* @max 0.25
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f);
|
||||
|
||||
/**
|
||||
* Spoiler landing setting
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
|
||||
|
||||
/**
|
||||
* Spoiler descend setting
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
|
||||
|
||||
/**
|
||||
* Spoiler input in manual flight
|
||||
*
|
||||
* Chose source for manual setting of spoilers in manual flight modes.
|
||||
*
|
||||
* @value 0 Disabled
|
||||
* @value 1 Flaps channel
|
||||
* @value 2 Aux1
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);
|
||||
|
||||
@@ -64,7 +64,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
|
||||
int32_t vt_type = -1;
|
||||
param_get(param_find("VT_TYPE"), &vt_type);
|
||||
|
||||
_is_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
}
|
||||
|
||||
// limit to 50 Hz
|
||||
@@ -290,7 +290,7 @@ FixedwingPositionControl::airspeed_poll()
|
||||
|
||||
airspeed_valid = true;
|
||||
|
||||
_time_airspeed_last_valid = airspeed_validated.timestamp;
|
||||
_airspeed_last_valid = airspeed_validated.timestamp;
|
||||
_airspeed = airspeed_validated.calibrated_airspeed_m_s;
|
||||
|
||||
_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
|
||||
@@ -298,7 +298,7 @@ FixedwingPositionControl::airspeed_poll()
|
||||
|
||||
} else {
|
||||
// no airspeed updates for one second
|
||||
if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
|
||||
if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
|
||||
airspeed_valid = false;
|
||||
}
|
||||
}
|
||||
@@ -370,7 +370,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
|
||||
|
||||
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
|
||||
// between multirotor and fixed wing flight
|
||||
if (_is_vtol_tailsitter) {
|
||||
if (_vtol_tailsitter) {
|
||||
const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
|
||||
R = R * R_offset;
|
||||
|
||||
@@ -755,28 +755,19 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
|
||||
|
||||
if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
|
||||
_control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
|
||||
|
||||
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
|
||||
|
||||
if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
|
||||
// TAKEOFF: handle like a regular POSITION setpoint if already flying
|
||||
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
|
||||
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
||||
|
||||
// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
|
||||
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
|
||||
} else {
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
|
||||
}
|
||||
|
||||
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
|
||||
if (!_vehicle_status.in_transition_mode) {
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
|
||||
|
||||
} else {
|
||||
// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
|
||||
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
}
|
||||
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
&& !_vehicle_status.in_transition_mode) {
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
|
||||
|
||||
} else {
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
||||
@@ -836,26 +827,58 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
||||
FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
|
||||
MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps
|
||||
|
||||
/* save time when airplane is in air */
|
||||
if (!_was_in_air && !_landed) {
|
||||
_was_in_air = true;
|
||||
_time_went_in_air = now;
|
||||
_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
|
||||
_takeoff_ground_alt = _current_altitude;
|
||||
}
|
||||
|
||||
/* reset flag when airplane landed */
|
||||
if (_landed) {
|
||||
_was_in_air = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp)
|
||||
{
|
||||
// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
|
||||
// shifting hacks
|
||||
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
|
||||
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
|
||||
/* reset integrators */
|
||||
_tecs.reset_state();
|
||||
}
|
||||
|
||||
/* reset hold yaw */
|
||||
_hdg_hold_yaw = _yaw;
|
||||
|
||||
/* get circle mode */
|
||||
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
|
||||
|
||||
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
|
||||
/* Initialize attitude controller integrator reset flags to 0 */
|
||||
_att_sp.roll_reset_integral = false;
|
||||
_att_sp.pitch_reset_integral = false;
|
||||
_att_sp.yaw_reset_integral = false;
|
||||
|
||||
position_setpoint_s current_sp = pos_sp_curr;
|
||||
|
||||
if (_vehicle_status.in_transition_to_fw) {
|
||||
|
||||
@@ -879,15 +902,6 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
|
||||
_transition_waypoint(0) = static_cast<double>(NAN);
|
||||
_transition_waypoint(1) = static_cast<double>(NAN);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
position_setpoint_s current_sp = pos_sp_curr;
|
||||
move_position_setpoint_for_vtol_transition(current_sp);
|
||||
|
||||
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
|
||||
|
||||
@@ -898,31 +912,36 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr
|
||||
publishOrbitStatus(current_sp);
|
||||
}
|
||||
|
||||
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
|
||||
|
||||
switch (position_sp_type) {
|
||||
case position_setpoint_s::SETPOINT_TYPE_IDLE:
|
||||
_att_sp.thrust_body[0] = 0.0f;
|
||||
_att_sp.roll_body = 0.0f;
|
||||
_att_sp.pitch_body = radians(_param_fw_psp_off.get());
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_POSITION:
|
||||
control_auto_position(now, control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
|
||||
control_auto_velocity(now, control_interval, curr_pos, ground_speed, pos_sp_prev);
|
||||
control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_LOITER:
|
||||
control_auto_loiter(now, control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
|
||||
control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
|
||||
break;
|
||||
}
|
||||
|
||||
/* reset landing state */
|
||||
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
reset_landing_state();
|
||||
}
|
||||
|
||||
/* reset takeoff/launch state */
|
||||
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
|
||||
reset_takeoff_state();
|
||||
}
|
||||
|
||||
if (was_circle_mode && !_l1_control.circle_mode()) {
|
||||
/* just kicked out of loiter, reset roll integrals */
|
||||
_att_sp.roll_reset_integral = true;
|
||||
@@ -949,6 +968,8 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr
|
||||
void
|
||||
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
|
||||
{
|
||||
// only control altitude and airspeed ("fixed-bank loiter")
|
||||
|
||||
tecs_update_pitch_throttle(now, _current_altitude,
|
||||
_param_fw_airspd_trim.get(),
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
@@ -970,14 +991,13 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
|
||||
}
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
|
||||
{
|
||||
// only control height rate
|
||||
|
||||
// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
|
||||
// but not letting it drift too far away.
|
||||
const float descend_rate = -0.5f;
|
||||
@@ -999,9 +1019,6 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
|
||||
|
||||
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
uint8_t
|
||||
@@ -1020,8 +1037,15 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
|
||||
|
||||
uint8_t position_sp_type = setpoint_type;
|
||||
|
||||
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|
||||
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
||||
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
|
||||
// TAKEOFF: handle like a regular POSITION setpoint if already flying
|
||||
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
|
||||
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
|
||||
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
}
|
||||
|
||||
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|
||||
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
||||
|
||||
float dist_xy = -1.f;
|
||||
float dist_z = -1.f;
|
||||
@@ -1058,13 +1082,17 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
|
||||
}
|
||||
}
|
||||
|
||||
// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
|
||||
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
|
||||
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
}
|
||||
|
||||
return position_sp_type;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr)
|
||||
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
|
||||
Vector2d curr_wp{0, 0};
|
||||
@@ -1117,7 +1145,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
// Altitude first order hold (FOH)
|
||||
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
|
||||
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
|
||||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
|
||||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
|
||||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
|
||||
) {
|
||||
const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
|
||||
pos_sp_prev.lat, pos_sp_prev.lon);
|
||||
@@ -1146,7 +1175,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
}
|
||||
}
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
|
||||
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
||||
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
|
||||
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
|
||||
@@ -1177,9 +1206,6 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
tecs_update_pitch_throttle(now, position_sp_alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
@@ -1192,8 +1218,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
|
||||
FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
float tecs_fw_thr_min;
|
||||
float tecs_fw_thr_max;
|
||||
@@ -1227,7 +1253,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
|
||||
_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
|
||||
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
||||
@@ -1243,9 +1269,6 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
|
||||
_att_sp.yaw_body = _yaw;
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
tecs_update_pitch_throttle(now, position_sp_alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
@@ -1260,8 +1283,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
@@ -1334,15 +1356,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
|
||||
// have to do this switch (which can cause significant altitude errors) close to the ground.
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
|
||||
|
||||
} else {
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
_att_sp.apply_flaps = true;
|
||||
}
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, control_interval);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
|
||||
|
||||
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
||||
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
|
||||
@@ -1395,10 +1412,20 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
|
||||
MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
Vector2d prev_wp{0, 0}; /* previous waypoint */
|
||||
@@ -1418,6 +1445,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
prev_wp(1) = pos_sp_curr.lon;
|
||||
}
|
||||
|
||||
// apply flaps for takeoff according to the corresponding scale factor set
|
||||
// via FW_FLAPS_TO_SCL
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
|
||||
|
||||
// continuously reset launch detection and runway takeoff until armed
|
||||
if (!_control_mode.flag_armed) {
|
||||
_launchDetector.reset();
|
||||
@@ -1445,7 +1476,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now,
|
||||
_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
|
||||
control_interval);
|
||||
dt);
|
||||
|
||||
/*
|
||||
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
|
||||
@@ -1457,9 +1488,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
||||
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
||||
// NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create
|
||||
// point following navigation interface instead of this hack.
|
||||
_npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
||||
_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
||||
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
@@ -1492,10 +1521,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
|
||||
_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
|
||||
|
||||
// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
} else {
|
||||
/* Perform launch detection */
|
||||
if (_launchDetector.launchDetectionEnabled() &&
|
||||
@@ -1512,7 +1537,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
/* Detect launch using body X (forward) acceleration */
|
||||
_launchDetector.update(control_interval, _body_acceleration(0));
|
||||
_launchDetector.update(dt, _body_acceleration(0));
|
||||
|
||||
/* update our copy of the launch detection state */
|
||||
_launch_detection_state = _launchDetector.getLaunchDetected();
|
||||
@@ -1528,7 +1553,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
/* Launch has been detected, hence we have to control the plane. */
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
|
||||
control_interval);
|
||||
dt);
|
||||
|
||||
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
|
||||
|
||||
@@ -1598,9 +1623,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_att_sp.roll_body = 0.0f;
|
||||
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
|
||||
}
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) {
|
||||
@@ -1631,12 +1653,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
// Enable tighter altitude control for landings
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
|
||||
MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
|
||||
@@ -1655,6 +1684,13 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
prev_wp(1) = pos_sp_curr.lon;
|
||||
}
|
||||
|
||||
// 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 = vehicle_attitude_setpoint_s::FLAPS_LAND;
|
||||
|
||||
// Enable tighter altitude control for landings
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
|
||||
// save time at which we started landing and reset abort_landing
|
||||
if (_time_started_landing == 0) {
|
||||
reset_landing_state();
|
||||
@@ -1732,10 +1768,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
// all good, have valid terrain altitude
|
||||
float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
|
||||
terrain_alt = (_local_pos.ref_alt - terrain_vpos);
|
||||
_last_valid_terrain_alt_estimate = terrain_alt;
|
||||
_last_time_terrain_alt_was_valid = now;
|
||||
_t_alt_prev_valid = terrain_alt;
|
||||
_time_last_t_alt = now;
|
||||
|
||||
} else if (_last_time_terrain_alt_was_valid == 0) {
|
||||
} 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
|
||||
@@ -1748,16 +1784,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
abort_landing(true);
|
||||
}
|
||||
|
||||
} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < T_ALT_TIMEOUT)
|
||||
} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
|
||||
|| _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 = _last_valid_terrain_alt_estimate;
|
||||
terrain_alt = _t_alt_prev_valid;
|
||||
|
||||
} else {
|
||||
// terrain alt was not valid for long time, abort landing
|
||||
terrain_alt = _last_valid_terrain_alt_estimate;
|
||||
terrain_alt = _t_alt_prev_valid;
|
||||
abort_landing(true);
|
||||
}
|
||||
}
|
||||
@@ -1800,7 +1836,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, control_interval);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
|
||||
|
||||
const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
|
||||
|
||||
@@ -1917,7 +1953,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, control_interval);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
|
||||
|
||||
/* lateral guidance */
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
@@ -1967,10 +2003,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
||||
// when we are landed state we want the motor to spin at idle speed
|
||||
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
|
||||
|
||||
// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
|
||||
|
||||
if (!_vehicle_status.in_transition_to_fw) {
|
||||
publishLocalPositionSetpoint(pos_sp_curr);
|
||||
}
|
||||
@@ -1981,6 +2013,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
|
||||
const Vector2f &ground_speed)
|
||||
{
|
||||
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
/* Get demanded airspeed */
|
||||
float altctrl_airspeed = get_manual_airspeed_setpoint();
|
||||
@@ -2035,18 +2068,16 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
|
||||
}
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
|
||||
// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
|
||||
// through attitdue setpoints
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed)
|
||||
{
|
||||
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
|
||||
MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
|
||||
// and set limit to pitch angle to prevent steering into ground
|
||||
// this will only affect planes and not VTOL
|
||||
@@ -2162,9 +2193,9 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
|
||||
float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
|
||||
const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
|
||||
|
||||
if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
|
||||
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
|
||||
_att_sp.roll_body + roll_rate_slew_rad * control_interval);
|
||||
if (dt > 0.f && roll_rate_slew_rad > 0.f) {
|
||||
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt,
|
||||
_att_sp.roll_body + roll_rate_slew_rad * dt);
|
||||
}
|
||||
|
||||
_att_sp.roll_body = roll_sp_new;
|
||||
@@ -2181,11 +2212,6 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
|
||||
}
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
|
||||
// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
|
||||
// through attitdue setpoints
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
float
|
||||
@@ -2225,10 +2251,6 @@ FixedwingPositionControl::Run()
|
||||
|
||||
if (_local_pos_sub.update(&_local_pos)) {
|
||||
|
||||
const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f,
|
||||
MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = _local_pos.timestamp;
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
@@ -2369,31 +2391,9 @@ FixedwingPositionControl::Run()
|
||||
|
||||
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
|
||||
|
||||
update_in_air_states(_local_pos.timestamp);
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(control_interval);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(control_interval);
|
||||
}
|
||||
|
||||
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
|
||||
_att_sp.roll_reset_integral = false;
|
||||
_att_sp.pitch_reset_integral = false;
|
||||
_att_sp.yaw_reset_integral = false;
|
||||
|
||||
// by default we don't want yaw to be contoller directly with rudder
|
||||
_att_sp.fw_control_yaw = false;
|
||||
|
||||
switch (_control_mode_current) {
|
||||
case FW_POSCTRL_MODE_AUTO: {
|
||||
control_auto(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
|
||||
_pos_sp_triplet.current,
|
||||
control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
|
||||
_pos_sp_triplet.next);
|
||||
break;
|
||||
}
|
||||
@@ -2409,19 +2409,17 @@ FixedwingPositionControl::Run()
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_LANDING: {
|
||||
control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
|
||||
_pos_sp_triplet.current);
|
||||
control_auto_landing(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
|
||||
control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
|
||||
_pos_sp_triplet.current);
|
||||
control_auto_takeoff(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_MANUAL_POSITION: {
|
||||
control_manual_position(_local_pos.timestamp, control_interval, curr_pos, ground_speed);
|
||||
control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -2439,24 +2437,11 @@ FixedwingPositionControl::Run()
|
||||
|
||||
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
|
||||
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
_tecs.reset_state();
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
|
||||
reset_landing_state();
|
||||
}
|
||||
|
||||
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
|
||||
reset_takeoff_state();
|
||||
}
|
||||
|
||||
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
|
||||
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
@@ -2517,7 +2502,7 @@ FixedwingPositionControl::reset_landing_state()
|
||||
_time_started_landing = 0;
|
||||
|
||||
// reset terrain estimation relevant values
|
||||
_last_time_terrain_alt_was_valid = 0;
|
||||
_time_last_t_alt = 0;
|
||||
|
||||
_land_noreturn_horizontal = false;
|
||||
_land_noreturn_vertical = false;
|
||||
@@ -2568,8 +2553,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
|
||||
bool climbout_mode, float climbout_pitch_min_rad,
|
||||
bool disable_underspeed_detection, float hgt_rate_sp)
|
||||
{
|
||||
const float dt = math::constrain((now - _time_last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
|
||||
_time_last_tecs_update = now;
|
||||
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
|
||||
_last_tecs_update = now;
|
||||
|
||||
// do not run TECS if we are not in air
|
||||
bool run_tecs = !_landed;
|
||||
@@ -2588,25 +2573,24 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
|
||||
// set this to transition airspeed to init tecs correctly
|
||||
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
|
||||
// some vtols fly without airspeed sensor
|
||||
_airspeed_after_transition = _param_airspeed_trans;
|
||||
_asp_after_transition = _param_airspeed_trans;
|
||||
|
||||
} else {
|
||||
_airspeed_after_transition = _airspeed;
|
||||
_asp_after_transition = _airspeed;
|
||||
}
|
||||
|
||||
_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
|
||||
_param_fw_airspd_max.get());
|
||||
_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
|
||||
|
||||
} else if (_was_in_transition) {
|
||||
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
|
||||
_airspeed_after_transition += dt * 2.0f; // increase 2m/s
|
||||
_asp_after_transition += dt * 2.0f; // increase 2m/s
|
||||
|
||||
if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
|
||||
airspeed_sp = max(_airspeed_after_transition, _airspeed);
|
||||
if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
|
||||
airspeed_sp = max(_asp_after_transition, _airspeed);
|
||||
|
||||
} else {
|
||||
_was_in_transition = false;
|
||||
_airspeed_after_transition = 0.0f;
|
||||
_asp_after_transition = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -103,38 +103,25 @@ using namespace time_literals;
|
||||
using matrix::Vector2d;
|
||||
using matrix::Vector2f;
|
||||
|
||||
// [m] initial distance of waypoint in front of plane in heading hold mode
|
||||
static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
|
||||
static constexpr float HDG_HOLD_DIST_NEXT =
|
||||
3000.0f; // initial distance of waypoint in front of plane in heading hold mode
|
||||
static constexpr float HDG_HOLD_REACHED_DIST =
|
||||
1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
|
||||
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
|
||||
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode
|
||||
static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
|
||||
0.01f; // max manual roll/yaw input from user which does not change the locked heading
|
||||
|
||||
// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
|
||||
static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;
|
||||
static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
|
||||
|
||||
// [m] distance by which previous waypoint is set behind the plane
|
||||
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
|
||||
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 ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
|
||||
static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
|
||||
10_s; // time after which the wind estimate is disabled if no longer updating
|
||||
|
||||
// [rad/s] max yawrate at which plane locks yaw for heading hold mode
|
||||
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
|
||||
|
||||
// [.] max manual roll/yaw normalized input from user which does not change the locked heading
|
||||
static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
|
||||
|
||||
// [us] time after which we abort landing if terrain estimate is not valid
|
||||
static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s;
|
||||
|
||||
// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
|
||||
static constexpr float THROTTLE_THRESH = 0.05f;
|
||||
|
||||
// [m/s/s] slew rate limit for airspeed setpoint changes
|
||||
static constexpr float ASPD_SP_SLEW_RATE = 1.f;
|
||||
|
||||
// [us] time after which the wind estimate is disabled if no longer updating
|
||||
static constexpr hrt_abstime T_WIND_EST_TIMEOUT = 10_s;
|
||||
|
||||
// [s] minimum time step between auto control updates
|
||||
static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
|
||||
|
||||
// [s] maximum time step between auto control updates
|
||||
static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
|
||||
static constexpr float MIN_AUTO_TIMESTEP = 0.01f; // minimum time step between auto control updates [s]
|
||||
static constexpr float MAX_AUTO_TIMESTEP = 0.05f; // maximum time step between auto control updates [s]
|
||||
|
||||
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
|
||||
public px4::WorkItem
|
||||
@@ -177,43 +164,39 @@ private:
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
|
||||
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
|
||||
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
|
||||
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};
|
||||
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
|
||||
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
|
||||
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
|
||||
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; ///< vehicle local position setpoint publication
|
||||
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
|
||||
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
|
||||
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
|
||||
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
|
||||
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
|
||||
|
||||
manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
|
||||
position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
|
||||
vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
|
||||
vehicle_control_mode_s _control_mode {};
|
||||
vehicle_local_position_s _local_pos {}; // vehicle local position
|
||||
vehicle_status_s _vehicle_status {}; // vehicle status
|
||||
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
|
||||
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items
|
||||
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
|
||||
vehicle_control_mode_s _control_mode {}; ///< control mode
|
||||
vehicle_local_position_s _local_pos {}; ///< vehicle local position
|
||||
vehicle_status_s _vehicle_status {}; ///< vehicle status
|
||||
|
||||
double _current_latitude{0};
|
||||
double _current_longitude{0};
|
||||
float _current_altitude{0.f};
|
||||
|
||||
perf_counter_t _loop_perf; // loop performance counter
|
||||
perf_counter_t _loop_perf; ///< loop performance counter
|
||||
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
// [m] ground altitude at which plane was launched
|
||||
float _takeoff_ground_alt{0.0f};
|
||||
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched
|
||||
float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode
|
||||
bool _hdg_hold_enabled{false}; ///< heading hold enabled
|
||||
bool _yaw_lock_engaged{false}; ///< yaw is locked for heading hold
|
||||
|
||||
// [rad] yaw setpoint for manual position mode heading hold
|
||||
float _hdg_hold_yaw{0.0f};
|
||||
float _min_current_sp_distance_xy{FLT_MAX};
|
||||
|
||||
bool _hdg_hold_enabled{false}; // heading hold enabled
|
||||
bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
|
||||
|
||||
float _min_current_sp_distance_xy{FLT_MAX};
|
||||
|
||||
position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
|
||||
position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
|
||||
position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started
|
||||
position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies
|
||||
|
||||
/**
|
||||
* @brief Last absolute time position control has been called [us]
|
||||
@@ -233,60 +216,38 @@ private:
|
||||
|
||||
Landingslope _landingslope;
|
||||
|
||||
// [us] time at which landing started
|
||||
hrt_abstime _time_started_landing{0};
|
||||
hrt_abstime _time_started_landing{0}; ///< time at which landing started
|
||||
|
||||
// [m] last terrain estimate which was valid
|
||||
float _last_valid_terrain_alt_estimate{0.0f};
|
||||
float _t_alt_prev_valid{0}; ///< last terrain estimate which was valid
|
||||
hrt_abstime _time_last_t_alt{0}; ///< time at which we had last valid terrain alt
|
||||
|
||||
// [us] time at which we had last valid terrain alt
|
||||
hrt_abstime _last_time_terrain_alt_was_valid{0};
|
||||
|
||||
// [m] estimated height to ground at which flare started
|
||||
float _flare_height{0.0f};
|
||||
|
||||
// [m] current forced (i.e. not determined using TECS) flare pitch setpoint
|
||||
float _flare_pitch_sp{0.0f};
|
||||
|
||||
// [m] estimated height to ground at which flare started
|
||||
float _flare_height{0.0f}; ///< estimated height to ground at which flare started
|
||||
float _flare_pitch_sp{0.0f}; ///< Current forced (i.e. not determined using TECS) flare pitch setpoint
|
||||
float _flare_curve_alt_rel_last{0.0f};
|
||||
float _target_bearing{0.0f}; ///< estimated height to ground at which flare started
|
||||
|
||||
float _target_bearing{0.0f}; // [rad]
|
||||
bool _was_in_air{false}; ///< indicated wether the plane was in the air in the previous interation*/
|
||||
hrt_abstime _time_went_in_air{0}; ///< time at which the plane went in the air
|
||||
|
||||
// indicates whether the plane was in the air in the previous interation
|
||||
bool _was_in_air{false};
|
||||
|
||||
// [us] time at which the plane went in the air
|
||||
hrt_abstime _time_went_in_air{0};
|
||||
|
||||
// Takeoff launch detection and runway
|
||||
/* Takeoff launch detection and runway */
|
||||
LaunchDetector _launchDetector;
|
||||
LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
|
||||
hrt_abstime _launch_detection_notify{0};
|
||||
|
||||
RunwayTakeoff _runway_takeoff;
|
||||
|
||||
// true if the last iteration was in manual mode (used to determine when a reset is needed)
|
||||
bool _last_manual{false};
|
||||
bool _last_manual{false}; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
|
||||
|
||||
/* throttle and airspeed states */
|
||||
|
||||
bool _airspeed_valid{false};
|
||||
|
||||
// [us] last time airspeed was received. used to detect timeouts.
|
||||
hrt_abstime _time_airspeed_last_valid{0};
|
||||
|
||||
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
|
||||
hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
|
||||
float _airspeed{0.0f};
|
||||
float _eas2tas{1.0f};
|
||||
|
||||
/* wind estimates */
|
||||
|
||||
// [m/s] wind velocity vector
|
||||
Vector2f _wind_vel{0.0f, 0.0f};
|
||||
|
||||
bool _wind_valid{false};
|
||||
|
||||
hrt_abstime _time_wind_last_received{0}; // [us]
|
||||
Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
|
||||
bool _wind_valid{false}; ///< flag if a valid wind estimate exists
|
||||
hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
|
||||
|
||||
float _pitch{0.0f};
|
||||
float _yaw{0.0f};
|
||||
@@ -295,40 +256,32 @@ private:
|
||||
matrix::Vector3f _body_acceleration{};
|
||||
matrix::Vector3f _body_velocity{};
|
||||
|
||||
bool _reinitialize_tecs{true};
|
||||
bool _reinitialize_tecs{true}; ///< indicates if the TECS states should be reinitialized (used for VTOL)
|
||||
bool _is_tecs_running{false};
|
||||
hrt_abstime _last_tecs_update{0};
|
||||
|
||||
hrt_abstime _time_last_tecs_update{0}; // [us]
|
||||
|
||||
float _airspeed_after_transition{0.0f};
|
||||
float _asp_after_transition{0.0f};
|
||||
bool _was_in_transition{false};
|
||||
|
||||
bool _is_vtol_tailsitter{false};
|
||||
bool _vtol_tailsitter{false};
|
||||
|
||||
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
|
||||
|
||||
// estimator reset counters
|
||||
uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position
|
||||
uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state
|
||||
|
||||
// captures the number of times the estimator has reset the horizontal position
|
||||
uint8_t _pos_reset_counter{0};
|
||||
float _manual_control_setpoint_altitude{0.0f};
|
||||
float _manual_control_setpoint_airspeed{0.0f};
|
||||
float _commanded_airspeed_setpoint{NAN}; ///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
|
||||
|
||||
// captures the number of times the estimator has reset the altitude state
|
||||
uint8_t _alt_reset_counter{0};
|
||||
hrt_abstime _time_in_fixed_bank_loiter{0};
|
||||
|
||||
float _manual_control_setpoint_altitude{0.0f}; // [m]
|
||||
float _manual_control_setpoint_airspeed{0.0f}; // [m/s]
|
||||
|
||||
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
|
||||
float _commanded_airspeed_setpoint{NAN};
|
||||
|
||||
hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
|
||||
|
||||
ECL_L1_Pos_Controller _l1_control;
|
||||
ECL_L1_Pos_Controller _l1_control;
|
||||
NPFG _npfg;
|
||||
TECS _tecs;
|
||||
TECS _tecs;
|
||||
|
||||
uint8_t _position_sp_type{0};
|
||||
|
||||
enum FW_POSCTRL_MODE {
|
||||
FW_POSCTRL_MODE_AUTO,
|
||||
FW_POSCTRL_MODE_AUTO_ALTITUDE,
|
||||
@@ -338,11 +291,10 @@ private:
|
||||
FW_POSCTRL_MODE_MANUAL_POSITION,
|
||||
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
|
||||
FW_POSCTRL_MODE_OTHER
|
||||
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
|
||||
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
|
||||
|
||||
param_t _param_handle_airspeed_trans{PARAM_INVALID};
|
||||
|
||||
float _param_airspeed_trans{NAN}; // [m/s]
|
||||
float _param_airspeed_trans{NAN};
|
||||
|
||||
enum StickConfig {
|
||||
STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
|
||||
@@ -350,27 +302,27 @@ private:
|
||||
};
|
||||
|
||||
// Update our local parameter cache.
|
||||
int parameters_update();
|
||||
int parameters_update();
|
||||
|
||||
// Update subscriptions
|
||||
void airspeed_poll();
|
||||
void control_update();
|
||||
void manual_control_setpoint_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_command_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
void vehicle_status_poll();
|
||||
void wind_poll();
|
||||
void airspeed_poll();
|
||||
void control_update();
|
||||
void manual_control_setpoint_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_command_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
void vehicle_status_poll();
|
||||
void wind_poll();
|
||||
|
||||
void status_publish();
|
||||
void landing_status_publish();
|
||||
void tecs_status_publish();
|
||||
void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint);
|
||||
void status_publish();
|
||||
void landing_status_publish();
|
||||
void tecs_status_publish();
|
||||
void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint);
|
||||
|
||||
void abort_landing(bool abort);
|
||||
void abort_landing(bool abort);
|
||||
|
||||
/**
|
||||
* @brief Get a new waypoint based on heading and distance from current position
|
||||
* Get a new waypoint based on heading and distance from current position
|
||||
*
|
||||
* @param heading the heading to fly to
|
||||
* @param distance the distance of the generated waypoint
|
||||
@@ -381,168 +333,72 @@ private:
|
||||
position_setpoint_s &waypoint_next, bool flag_init);
|
||||
|
||||
/**
|
||||
* @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
|
||||
* Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
|
||||
*/
|
||||
float get_terrain_altitude_takeoff(float takeoff_alt);
|
||||
float get_terrain_altitude_takeoff(float takeoff_alt);
|
||||
|
||||
float getManualHeightRateSetpoint();
|
||||
|
||||
/**
|
||||
* @brief Check if we are in a takeoff situation
|
||||
* Check if we are in a takeoff situation
|
||||
*/
|
||||
bool in_takeoff_situation();
|
||||
bool in_takeoff_situation();
|
||||
|
||||
/**
|
||||
* @brief Update desired altitude base on user pitch stick input
|
||||
* Update desired altitude base on user pitch stick input
|
||||
*
|
||||
* @param dt Time step
|
||||
*/
|
||||
void update_desired_altitude(float dt);
|
||||
void update_desired_altitude(float dt);
|
||||
uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
|
||||
void control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
||||
|
||||
void control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
|
||||
void control_auto_descend(const hrt_abstime &now);
|
||||
|
||||
void control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
void control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
||||
void control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Updates timing information for landed and in-air states.
|
||||
* @brief Vehicle control while in takeoff
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
*/
|
||||
void update_in_air_states(const hrt_abstime now);
|
||||
|
||||
/**
|
||||
* @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
|
||||
* transition.
|
||||
*
|
||||
* @param[in,out] current_sp current position setpoint
|
||||
*/
|
||||
void move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp);
|
||||
|
||||
uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Position control for all automatic modes except takeoff and landing
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
* @param pos_sp_next next position setpoint
|
||||
*/
|
||||
void control_auto(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next);
|
||||
|
||||
/**
|
||||
* @brief Controls altitude and airspeed for a fixed-bank loiter.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
*/
|
||||
void control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
|
||||
|
||||
/**
|
||||
* @brief Control airspeed with a fixed descent rate and roll angle.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
*/
|
||||
void control_auto_descend(const hrt_abstime &now);
|
||||
|
||||
/**
|
||||
* @brief Vehicle control for position waypoints.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_position(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
void control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed,
|
||||
const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr);
|
||||
void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
|
||||
void control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* @brief Vehicle control for loiter waypoints.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
* @param pos_sp_next next position setpoint
|
||||
*/
|
||||
void control_auto_loiter(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next);
|
||||
float get_tecs_pitch();
|
||||
float get_tecs_thrust();
|
||||
|
||||
/**
|
||||
* @brief Controls a desired airspeed, bearing, and height rate.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_velocity(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
|
||||
float get_manual_airspeed_setpoint();
|
||||
float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
|
||||
float dt);
|
||||
|
||||
/**
|
||||
* @brief Controls automatic takeoff.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls automatic landing.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
* @param pos_sp_prev previous position setpoint
|
||||
* @param pos_sp_curr current position setpoint
|
||||
*/
|
||||
void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
|
||||
|
||||
/**
|
||||
* @brief Controls altitude and airspeed, user commands roll setpoint.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
*/
|
||||
void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
|
||||
|
||||
/**
|
||||
* @brief Controls user commanded altitude, airspeed, and bearing.
|
||||
*
|
||||
* @param now Current system time [us]
|
||||
* @param control_interval Time since last position control call [s]
|
||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
||||
*/
|
||||
void control_manual_position(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed);
|
||||
|
||||
float get_tecs_pitch();
|
||||
float get_tecs_thrust();
|
||||
|
||||
float get_manual_airspeed_setpoint();
|
||||
float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed,
|
||||
float dt);
|
||||
|
||||
void reset_takeoff_state(bool force = false);
|
||||
void reset_landing_state();
|
||||
bool using_npfg_with_wind_estimate() const;
|
||||
Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
|
||||
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
|
||||
void reset_takeoff_state(bool force = false);
|
||||
void reset_landing_state();
|
||||
bool using_npfg_with_wind_estimate() const;
|
||||
Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
|
||||
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
|
||||
|
||||
void publishOrbitStatus(const position_setpoint_s pos_sp);
|
||||
|
||||
|
||||
@@ -88,7 +88,7 @@ public:
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
TakeoffHandling _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
|
||||
Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
|
||||
@@ -39,14 +39,14 @@
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
void TakeoffHandling::generateInitialRampValue(float velocity_p_gain)
|
||||
void Takeoff::generateInitialRampValue(float velocity_p_gain)
|
||||
{
|
||||
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
|
||||
_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
|
||||
}
|
||||
|
||||
void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
|
||||
const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
|
||||
void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
|
||||
const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us)
|
||||
{
|
||||
_spoolup_time_hysteresis.set_state_and_update(armed, now_us);
|
||||
|
||||
@@ -109,7 +109,7 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co
|
||||
}
|
||||
}
|
||||
|
||||
float TakeoffHandling::updateRamp(const float dt, const float takeoff_desired_vz)
|
||||
float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz)
|
||||
{
|
||||
float upwards_velocity_limit = takeoff_desired_vz;
|
||||
|
||||
|
||||
@@ -53,11 +53,11 @@ enum class TakeoffState {
|
||||
flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT
|
||||
};
|
||||
|
||||
class TakeoffHandling
|
||||
class Takeoff
|
||||
{
|
||||
public:
|
||||
TakeoffHandling() = default;
|
||||
~TakeoffHandling() = default;
|
||||
Takeoff() = default;
|
||||
~Takeoff() = default;
|
||||
|
||||
// initialize parameters
|
||||
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); }
|
||||
|
||||
@@ -38,13 +38,13 @@
|
||||
|
||||
TEST(TakeoffTest, Initialization)
|
||||
{
|
||||
TakeoffHandling takeoff;
|
||||
Takeoff takeoff;
|
||||
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
|
||||
}
|
||||
|
||||
TEST(TakeoffTest, RegularTakeoffRamp)
|
||||
{
|
||||
TakeoffHandling takeoff;
|
||||
Takeoff takeoff;
|
||||
takeoff.setSpoolupTime(1.f);
|
||||
takeoff.setTakeoffRampTime(2.0);
|
||||
takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
|
||||
|
||||
@@ -39,7 +39,5 @@ px4_add_module(
|
||||
vtol_type.cpp
|
||||
tailsitter.cpp
|
||||
standard.cpp
|
||||
DEPENDS
|
||||
SlewRate
|
||||
)
|
||||
|
||||
|
||||
@@ -288,10 +288,6 @@ void Standard::update_transition_state()
|
||||
}
|
||||
}
|
||||
|
||||
// set spoiler and flaps to 0
|
||||
_flaps_setpoint_with_slewrate.update(0.f, _dt);
|
||||
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
|
||||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
|
||||
|
||||
if (_v_control_mode->flag_control_climb_rate_enabled) {
|
||||
@@ -374,9 +370,8 @@ void Standard::fill_actuator_outputs()
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH];
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0;
|
||||
|
||||
break;
|
||||
|
||||
@@ -396,8 +391,7 @@ void Standard::fill_actuator_outputs()
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
|
||||
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS];
|
||||
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output;
|
||||
|
||||
break;
|
||||
@@ -415,8 +409,7 @@ void Standard::fill_actuator_outputs()
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
|
||||
fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS];
|
||||
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -370,10 +370,6 @@ void Tiltrotor::update_transition_state()
|
||||
// add minimum throttle for front transition
|
||||
_thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN);
|
||||
|
||||
// set spoiler and flaps to 0
|
||||
_flaps_setpoint_with_slewrate.update(0.f, _dt);
|
||||
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
|
||||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
|
||||
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
|
||||
_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
|
||||
@@ -396,10 +392,6 @@ void Tiltrotor::update_transition_state()
|
||||
// if this is not then then there is race condition where the fw rate controller still publishes a zero sample throttle after transition
|
||||
_v_att_sp->thrust_body[0] = _thrust_transition;
|
||||
|
||||
// set spoiler and flaps to 0
|
||||
_flaps_setpoint_with_slewrate.update(0.f, _dt);
|
||||
_spoiler_setpoint_with_slewrate.update(0.f, _dt);
|
||||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
|
||||
|
||||
// set idle speed for rotary wing mode
|
||||
@@ -539,12 +531,12 @@ void Tiltrotor::fill_actuator_outputs()
|
||||
}
|
||||
|
||||
// Fixed wing output
|
||||
fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
|
||||
fw_out[4] = _tilt_control;
|
||||
|
||||
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
|
||||
} else {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
|
||||
@@ -555,10 +547,6 @@ void Tiltrotor::fill_actuator_outputs()
|
||||
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
|
||||
}
|
||||
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0;
|
||||
|
||||
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
|
||||
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
|
||||
|
||||
|
||||
@@ -109,7 +109,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
|
||||
_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
|
||||
|
||||
_params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD");
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
@@ -373,8 +372,6 @@ VtolAttitudeControl::parameters_update()
|
||||
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
|
||||
param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
|
||||
|
||||
param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld);
|
||||
|
||||
// update the parameters of the instances of base VtolType
|
||||
if (_vtol_type != nullptr) {
|
||||
_vtol_type->parameters_update();
|
||||
@@ -404,7 +401,6 @@ VtolAttitudeControl::Run()
|
||||
|
||||
#endif // !ENABLE_LOCKSTEP_SCHEDULER
|
||||
|
||||
const float dt = math::min((now - _last_run_timestamp) / 1e6f, kMaxVTOLAttitudeControlTimeStep);
|
||||
_last_run_timestamp = now;
|
||||
|
||||
if (!_initialized) {
|
||||
@@ -419,8 +415,6 @@ VtolAttitudeControl::Run()
|
||||
}
|
||||
}
|
||||
|
||||
_vtol_type->setDt(dt);
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in);
|
||||
|
||||
@@ -92,8 +92,6 @@ using namespace time_literals;
|
||||
|
||||
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
|
||||
|
||||
static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s]
|
||||
|
||||
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
@@ -251,7 +249,6 @@ private:
|
||||
param_t mpc_land_alt1;
|
||||
param_t mpc_land_alt2;
|
||||
param_t sys_ctrl_alloc;
|
||||
param_t vt_spoiler_mc_ld;
|
||||
} _params_handles{};
|
||||
|
||||
hrt_abstime _last_run_timestamp{0};
|
||||
|
||||
@@ -384,15 +384,3 @@ PARAM_DEFINE_FLOAT(VT_PTCH_MIN, -5.0f);
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_LND_PTCH_MIN, -5.0f);
|
||||
|
||||
/**
|
||||
* Spoiler setting while landing (hover)
|
||||
*
|
||||
* @unit norm
|
||||
* @min -1
|
||||
* @max 1
|
||||
* @decimal 1
|
||||
* @increment 0.05
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_SPOILER_MC_LD, 0.f);
|
||||
|
||||
@@ -136,9 +136,6 @@ bool VtolType::init()
|
||||
}
|
||||
}
|
||||
|
||||
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol);
|
||||
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol);
|
||||
|
||||
return true;
|
||||
|
||||
}
|
||||
@@ -160,16 +157,6 @@ void VtolType::update_mc_state()
|
||||
_mc_pitch_weight = 1.0f;
|
||||
_mc_yaw_weight = 1.0f;
|
||||
_mc_throttle_weight = 1.0f;
|
||||
|
||||
float spoiler_setpoint_hover = 0.f;
|
||||
|
||||
if (_attc->get_pos_sp_triplet()->current.valid
|
||||
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
spoiler_setpoint_hover = _params->vt_spoiler_mc_ld;
|
||||
}
|
||||
|
||||
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_setpoint_hover, 0.f, 1.f), _dt);
|
||||
_flaps_setpoint_with_slewrate.update(0.f, _dt);
|
||||
}
|
||||
|
||||
void VtolType::update_fw_state()
|
||||
@@ -218,9 +205,6 @@ void VtolType::update_fw_state()
|
||||
}
|
||||
|
||||
check_quadchute_condition();
|
||||
|
||||
_spoiler_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_SPOILERS], _dt);
|
||||
_flaps_setpoint_with_slewrate.update(_actuators_fw_in->control[actuator_controls_s::INDEX_FLAPS], _dt);
|
||||
}
|
||||
|
||||
void VtolType::update_transition_state()
|
||||
@@ -231,6 +215,8 @@ void VtolType::update_transition_state()
|
||||
_last_loop_ts = t_now;
|
||||
_throttle_blend_start_ts = t_now;
|
||||
|
||||
|
||||
|
||||
check_quadchute_condition();
|
||||
}
|
||||
|
||||
|
||||
@@ -46,10 +46,6 @@
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
|
||||
static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s]
|
||||
static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s]
|
||||
|
||||
struct Params {
|
||||
int32_t ctrl_alloc;
|
||||
@@ -85,7 +81,6 @@ struct Params {
|
||||
int32_t vt_forward_thrust_enable_mode;
|
||||
float mpc_land_alt1;
|
||||
float mpc_land_alt2;
|
||||
float vt_spoiler_mc_ld;
|
||||
};
|
||||
|
||||
// Has to match 1:1 msg/vtol_vehicle_status.msg
|
||||
@@ -212,14 +207,6 @@ public:
|
||||
|
||||
virtual void parameters_update() = 0;
|
||||
|
||||
/**
|
||||
* @brief Set current time delta
|
||||
*
|
||||
* @param dt Current time delta [s]
|
||||
*/
|
||||
void setDt(float dt) {_dt = dt; }
|
||||
|
||||
protected:
|
||||
VtolAttitudeControl *_attc;
|
||||
mode _vtol_mode;
|
||||
|
||||
@@ -305,11 +292,6 @@ protected:
|
||||
|
||||
float update_and_get_backtransition_pitch_sp();
|
||||
|
||||
SlewRate<float> _spoiler_setpoint_with_slewrate;
|
||||
SlewRate<float> _flaps_setpoint_with_slewrate;
|
||||
|
||||
float _dt{0.0025f}; // time step [s]
|
||||
|
||||
private:
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user