forked from Archive/PX4-Autopilot
create new multicopter rate controller module (mc_rate_control) split out of mc_att_control
This commit is contained in:
Daniel Agar
GitHub
parent
b64abf48b2
commit
84fe64b1c2
|
|
@ -48,6 +48,11 @@ fi
|
|||
# End Estimator Group Selection #
|
||||
###############################################################################
|
||||
|
||||
#
|
||||
# Start Multicopter Rate Controller.
|
||||
#
|
||||
mc_rate_control start
|
||||
|
||||
#
|
||||
# Start Multicopter Attitude Controller.
|
||||
#
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@ ekf2 start
|
|||
|
||||
|
||||
vtol_att_control start
|
||||
mc_rate_control start
|
||||
mc_att_control start
|
||||
mc_pos_control start
|
||||
fw_att_control start
|
||||
|
|
|
|||
|
|
@ -243,7 +243,7 @@ class Graph(object):
|
|||
|
||||
('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
('mc_rate_control', r'MulticopterRateControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'),
|
||||
|
||||
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ px4_add_board(
|
|||
landing_target_estimator
|
||||
local_position_estimator
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
sensors
|
||||
#sih
|
||||
|
|
|
|||
|
|
@ -80,6 +80,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ px4_add_board(
|
|||
landing_target_estimator
|
||||
local_position_estimator
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
sensors
|
||||
sih
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -71,6 +71,7 @@ px4_add_board(
|
|||
mavlink
|
||||
mc_att_control
|
||||
mc_pos_control
|
||||
mc_rate_control
|
||||
navigator
|
||||
rover_pos_control
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -72,6 +72,7 @@ px4_add_board(
|
|||
mavlink
|
||||
mc_att_control
|
||||
mc_pos_control
|
||||
mc_rate_control
|
||||
navigator
|
||||
battery_status
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
sensors
|
||||
|
|
|
|||
|
|
@ -78,6 +78,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ px4_add_board(
|
|||
irlock
|
||||
lights/rgbled
|
||||
magnetometer/hmc5883
|
||||
optical_flow/px4flow
|
||||
#optical_flow/px4flow
|
||||
px4fmu
|
||||
px4io
|
||||
tone_alarm
|
||||
|
|
@ -52,6 +52,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
@ -81,6 +82,6 @@ px4_add_board(
|
|||
tune_control
|
||||
usb_connected
|
||||
ver
|
||||
work_queue
|
||||
#work_queue
|
||||
|
||||
)
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -77,6 +77,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -66,6 +66,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
rover_pos_control
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -66,6 +66,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
rover_pos_control
|
||||
|
|
|
|||
|
|
@ -71,6 +71,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ px4_add_board(
|
|||
adc
|
||||
barometer/ms5611
|
||||
#batt_smbus
|
||||
camera_capture
|
||||
camera_trigger
|
||||
#camera_capture
|
||||
#camera_trigger
|
||||
differential_pressure # all available differential pressure drivers
|
||||
distance_sensor # all available distance sensor drivers
|
||||
gps
|
||||
|
|
@ -42,6 +42,7 @@ px4_add_board(
|
|||
mkblctrl
|
||||
optical_flow # all available optical flow drivers
|
||||
#pca9685
|
||||
#power_monitor/ina226
|
||||
#protocol_splitter
|
||||
pwm_input
|
||||
pwm_out_sim
|
||||
|
|
@ -73,6 +74,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
#micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
rover_pos_control
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
battery_status
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
replay
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
micrortps_bridge
|
||||
navigator
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@ px4_add_board(
|
|||
logger
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_rate_control
|
||||
mc_pos_control
|
||||
navigator
|
||||
replay
|
||||
|
|
|
|||
|
|
@ -55,6 +55,7 @@ px4_add_board(
|
|||
mavlink
|
||||
mc_att_control
|
||||
mc_pos_control
|
||||
mc_rate_control
|
||||
navigator
|
||||
sensors
|
||||
sih
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@ const char *get_commands()
|
|||
"ekf2 start\n"
|
||||
"mc_pos_control start\n"
|
||||
"mc_att_control start\n"
|
||||
"mc_rate_control start\n"
|
||||
"sleep 1\n"
|
||||
"pwm_out_sim start\n"
|
||||
"param set RC1_MAX 2015\n"
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ ekf2 start
|
|||
vtol_att_control start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
fw_pos_control_l1 start
|
||||
fw_att_control start
|
||||
airspeed_selector start
|
||||
|
|
@ -54,6 +55,7 @@ navigator status
|
|||
ekf2 status
|
||||
mc_pos_control status
|
||||
mc_att_control status
|
||||
mc_rate_control status
|
||||
fw_pos_control_l1 status
|
||||
fw_att_control status
|
||||
airspeed_selector status
|
||||
|
|
@ -64,6 +66,7 @@ uorb status
|
|||
echo "Stopping all modules"
|
||||
logger stop
|
||||
pwm_out_sim stop
|
||||
mc_rate_control stop
|
||||
mc_att_control stop
|
||||
fw_att_control stop
|
||||
mc_pos_control stop
|
||||
|
|
|
|||
|
|
@ -22,10 +22,10 @@ param set SYS_AUTOSTART 4011
|
|||
# Note that the setting here applies to all PWM channels.
|
||||
# param set PWM_MIN 1120
|
||||
# param set PWM_MAX 1920
|
||||
# Not using DJI 430 LITE ESC anymore due to its hiccups:
|
||||
# Not using DJI 430 LITE ESC anymore due to its hiccups:
|
||||
# each random motor stop would cause a scary flip in the fly
|
||||
# Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting
|
||||
|
||||
|
||||
# Broadcast heartbeats on local network. This allows a ground control station
|
||||
# to automatically find the drone on the local network.
|
||||
param set MAV_BROADCAST 1
|
||||
|
|
@ -70,10 +70,11 @@ land_detector start multicopter
|
|||
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
#fw_att_control start
|
||||
#fw_pos_control_l1 start
|
||||
|
||||
mavlink start -n SoftAp -x -u 14556 -r 1000000
|
||||
mavlink start -n SoftAp -x -u 14556 -r 1000000
|
||||
# -n name of wifi interface: SoftAp, wlan or your custom interface,
|
||||
# e.g., -n SoftAp . The default is wlan
|
||||
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ param set SYS_AUTOSTART 4011
|
|||
# Not using DJI 430 LITE ESC anymore due to its hiccups:
|
||||
# each random motor stop would cause a scary flip in the fly
|
||||
# Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting
|
||||
|
||||
|
||||
# Broadcast heartbeats on local network. This allows a ground control station
|
||||
# to automatically find the drone on the local network.
|
||||
param set MAV_BROADCAST 1
|
||||
|
|
@ -36,7 +36,7 @@ param set MAV_TYPE 2
|
|||
# Three possible main power battery sources for BBBlue:
|
||||
# 1. onboard 2S LiPo battery connector, which is connect to ADC channel 6
|
||||
# 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default.
|
||||
# 3. other power source (e.g., LiPo battery more than 4S/18V).
|
||||
# 3. other power source (e.g., LiPo battery more than 4S/18V).
|
||||
# Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3.
|
||||
param set BAT_ADC_CHANNEL 5
|
||||
|
||||
|
|
@ -70,11 +70,12 @@ ekf2 start
|
|||
|
||||
#mc_pos_control start
|
||||
#mc_att_control start
|
||||
#mc_rate_control start
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
mavlink start -n SoftAp -x -u 14556 -r 1000000
|
||||
# -n name of wifi interface: SoftAp, wlan or your custom interface,
|
||||
mavlink start -n SoftAp -x -u 14556 -r 1000000
|
||||
# -n name of wifi interface: SoftAp, wlan or your custom interface,
|
||||
# e.g., -n wlan . The default on BBBlue is SoftAp
|
||||
|
||||
sleep 1
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@ navigator start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
sleep 1
|
||||
mavlink start -x -u 14556 -r 20000
|
||||
sleep 1
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
uart_esc start -D /dev/tty-2
|
||||
spektrum_rc start -d /dev/tty-1
|
||||
sleep 1
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
uart_esc start -D /dev/tty-2
|
||||
spektrum_rc start -d /dev/tty-1
|
||||
sleep 1
|
||||
|
|
|
|||
|
|
@ -18,5 +18,6 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
snapdragon_pwm_out start
|
||||
spektrum_rc start
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ sensors start
|
|||
ekf2 start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
land_detector start multicopter
|
||||
sleep 1
|
||||
pwm_out_sim start
|
||||
|
|
|
|||
|
|
@ -197,6 +197,7 @@ qshell commander start
|
|||
qshell land_detector start multicopter
|
||||
qshell mc_pos_control start
|
||||
qshell mc_att_control start
|
||||
qshell mc_rate_control start
|
||||
|
||||
logger start -b 200 -t
|
||||
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
uart_esc start -D /dev/tty-1
|
||||
spektrum_rc start -d /dev/tty-101
|
||||
sleep 1
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
|
||||
mavlink start -x -d /dev/ttyPS1
|
||||
mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
mavlink start -x -u 14556 -r 1000000
|
||||
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
|
||||
mavlink stream -u 14556 -s ATTITUDE -r 50
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
mavlink start -x -u 14577 -r 1000000
|
||||
navio_sysfs_rc_in start
|
||||
pwm_out_sim start
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@ ekf2 start
|
|||
land_detector start multicopter
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
mavlink start -x -u 14556 -r 1000000
|
||||
sleep 1
|
||||
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ int ex_hwtest_main(int argc, char *argv[])
|
|||
{
|
||||
warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!");
|
||||
warnx("(run <commander stop>,)");
|
||||
warnx("( <mc_att_control stop> and)");
|
||||
warnx("( <mc_rate_control stop> and)");
|
||||
warnx("( <fw_att_control stop> to do so)");
|
||||
warnx("usage: http://px4.io/dev/examples/write_output");
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
|
@ -32,20 +32,15 @@
|
|||
############################################################################
|
||||
|
||||
add_subdirectory(AttitudeControl)
|
||||
add_subdirectory(RateControl)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__mc_att_control
|
||||
MAIN mc_att_control
|
||||
STACK_MAX 3500
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
mc_att_control_main.cpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
conversion
|
||||
mathlib
|
||||
AttitudeControl
|
||||
RateControl
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -31,6 +31,8 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
|
|
@ -41,27 +43,19 @@
|
|||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <vtol_att_control/vtol_type.h>
|
||||
|
||||
#include <AttitudeControl.hpp>
|
||||
#include <RateControl.hpp>
|
||||
|
||||
/**
|
||||
* Multicopter attitude control app start / stop handling function
|
||||
|
|
@ -99,17 +93,9 @@ private:
|
|||
*/
|
||||
void parameters_updated();
|
||||
|
||||
/**
|
||||
* Check for parameter update and handle it.
|
||||
*/
|
||||
void parameter_update_poll();
|
||||
bool vehicle_attitude_poll();
|
||||
void vehicle_motor_limits_poll();
|
||||
void vehicle_status_poll();
|
||||
|
||||
void publish_actuator_controls();
|
||||
void publish_rates_setpoint();
|
||||
void publish_rate_controller_status();
|
||||
|
||||
float throttle_curve(float throttle_stick_input);
|
||||
|
||||
|
|
@ -118,130 +104,60 @@ private:
|
|||
*/
|
||||
void generate_attitude_setpoint(float dt, bool reset_yaw_sp);
|
||||
|
||||
/**
|
||||
* Get the landing gear state based on the manual control switch position
|
||||
* @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
|
||||
*/
|
||||
float get_landing_gear_state();
|
||||
|
||||
|
||||
/**
|
||||
* Attitude controller.
|
||||
*/
|
||||
void control_attitude();
|
||||
|
||||
/**
|
||||
* Attitude rates controller.
|
||||
*/
|
||||
void control_attitude_rates(float dt, const matrix::Vector3f &rates);
|
||||
|
||||
AttitudeControl _attitude_control; ///< class for attitude control calculations
|
||||
RateControl _rate_control; ///< class for rate control calculations
|
||||
|
||||
uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */
|
||||
uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
|
||||
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
|
||||
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
|
||||
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
|
||||
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
|
||||
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
|
||||
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
|
||||
|
||||
orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */
|
||||
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr};
|
||||
|
||||
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
|
||||
orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */
|
||||
|
||||
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
|
||||
|
||||
struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */
|
||||
struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */
|
||||
struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */
|
||||
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
|
||||
struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */
|
||||
struct actuator_controls_s _actuators {}; /**< actuator controls */
|
||||
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
|
||||
struct battery_status_s _battery_status {}; /**< battery status */
|
||||
struct vehicle_land_detected_s _vehicle_land_detected {};
|
||||
struct landing_gear_s _landing_gear {};
|
||||
|
||||
MultirotorMixer::saturation_status _saturation_status{};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */
|
||||
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
|
||||
perf_counter_t _loop_perf; /**< loop duration performance counter */
|
||||
|
||||
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
|
||||
|
||||
matrix::Vector3f _att_control; /**< attitude control vector */
|
||||
float _thrust_sp{0.0f}; /**< thrust setpoint */
|
||||
|
||||
float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */
|
||||
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
|
||||
|
||||
hrt_abstime _task_start{hrt_absolute_time()};
|
||||
hrt_abstime _last_run{0};
|
||||
float _dt_accumulator{0.0f};
|
||||
int _loop_counter{0};
|
||||
|
||||
bool _reset_yaw_sp{true};
|
||||
float _attitude_dt{0.0f};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
|
||||
(ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_K>) _param_mc_rollrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
|
||||
(ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_K>) _param_mc_pitchrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
|
||||
(ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
|
||||
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
|
||||
|
||||
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
|
||||
|
||||
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
|
||||
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
|
||||
|
||||
(ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
|
||||
|
||||
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
|
||||
|
||||
/* Stabilized mode params */
|
||||
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
|
||||
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
|
||||
|
|
@ -250,15 +166,11 @@ private:
|
|||
_param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
|
||||
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
|
||||
|
||||
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
|
||||
|
||||
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
|
||||
|
||||
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
|
||||
)
|
||||
|
||||
bool _is_tailsitter{false};
|
||||
|
||||
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
|
||||
float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -45,10 +45,7 @@
|
|||
|
||||
#include "mc_att_control.hpp"
|
||||
|
||||
#include <conversion/rotation.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
|
||||
|
|
@ -56,8 +53,8 @@ using namespace matrix;
|
|||
|
||||
MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
|
||||
|
|
@ -65,10 +62,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|||
_v_att.q[0] = 1.f;
|
||||
_v_att_sp.q_d[0] = 1.f;
|
||||
|
||||
_rates_sp.zero();
|
||||
_thrust_sp = 0.0f;
|
||||
_att_control.zero();
|
||||
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
|
|
@ -80,8 +73,8 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
|
|||
bool
|
||||
MulticopterAttitudeControl::init()
|
||||
{
|
||||
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle_angular_velocity callback registration failed!");
|
||||
if (!_vehicle_attitude_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle_attitude callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -94,47 +87,12 @@ MulticopterAttitudeControl::parameters_updated()
|
|||
// Store some of the parameters in a more convenient way & precompute often-used values
|
||||
_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()));
|
||||
|
||||
// rate control parameters
|
||||
// The controller gain K is used to convert the parallel (P + I/s + sD) form
|
||||
// to the ideal (K * [1 + 1/sTi + sTd]) form
|
||||
Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
|
||||
_rate_control.setGains(
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));
|
||||
_rate_control.setIntegratorLimit(
|
||||
Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
|
||||
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false);
|
||||
_rate_control.setFeedForwardGain(
|
||||
Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
|
||||
|
||||
// angular rate limits
|
||||
using math::radians;
|
||||
_attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()),
|
||||
radians(_param_mc_yawrate_max.get())));
|
||||
|
||||
// manual rate control acro mode rate limits
|
||||
_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
|
||||
radians(_param_mc_acro_y_max.get()));
|
||||
|
||||
_man_tilt_max = math::radians(_param_mpc_man_tilt_max.get());
|
||||
|
||||
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::parameter_update_poll()
|
||||
{
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -143,9 +101,8 @@ MulticopterAttitudeControl::vehicle_status_poll()
|
|||
/* check if there is new status information */
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (_actuators_id == nullptr) {
|
||||
if (_attitude_sp_id == nullptr) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
|
||||
_attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);
|
||||
|
||||
int32_t vt_type = -1;
|
||||
|
|
@ -155,43 +112,12 @@ MulticopterAttitudeControl::vehicle_status_poll()
|
|||
}
|
||||
|
||||
} else {
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
_attitude_sp_id = ORB_ID(vehicle_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::vehicle_motor_limits_poll()
|
||||
{
|
||||
/* check if there is a new message */
|
||||
multirotor_motor_limits_s motor_limits{};
|
||||
|
||||
if (_motor_limits_sub.update(&motor_limits)) {
|
||||
_saturation_status.value = motor_limits.saturation_status;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
MulticopterAttitudeControl::vehicle_attitude_poll()
|
||||
{
|
||||
/* check if there is a new message */
|
||||
const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
|
||||
|
||||
if (_v_att_sub.update(&_v_att)) {
|
||||
// Check for a heading reset
|
||||
if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
|
||||
// we only extract the heading change from the delta quaternion
|
||||
_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
float
|
||||
MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
|
||||
{
|
||||
|
|
@ -214,30 +140,6 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
|
|||
}
|
||||
}
|
||||
|
||||
float
|
||||
MulticopterAttitudeControl::get_landing_gear_state()
|
||||
{
|
||||
// Only switch the landing gear up if we are not landed and if
|
||||
// the user switched from gear down to gear up.
|
||||
// If the user had the switch in the gear up position and took off ignore it
|
||||
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
|
||||
if (_vehicle_land_detected.landed) {
|
||||
_gear_state_initialized = false;
|
||||
}
|
||||
|
||||
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
|
||||
|
||||
if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
|
||||
landing_gear = landing_gear_s::GEAR_UP;
|
||||
|
||||
} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
|
||||
// Switching the gear off does put it into a safe defined state
|
||||
_gear_state_initialized = true;
|
||||
}
|
||||
|
||||
return landing_gear;
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
|
||||
{
|
||||
|
|
@ -334,16 +236,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
|
|||
if (_attitude_sp_id != nullptr) {
|
||||
orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
_landing_gear.landing_gear = get_landing_gear_state();
|
||||
_landing_gear.timestamp = hrt_absolute_time();
|
||||
_landing_gear_pub.publish(_landing_gear);
|
||||
}
|
||||
|
||||
/**
|
||||
* Attitude controller.
|
||||
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
|
||||
* Output: '_rates_sp' vector, '_thrust_sp'
|
||||
* Output: '_rates_sp' vector
|
||||
*/
|
||||
void
|
||||
MulticopterAttitudeControl::control_attitude()
|
||||
|
|
@ -356,124 +254,72 @@ MulticopterAttitudeControl::control_attitude()
|
|||
Vector3f().copyTo(_v_att_sp.thrust_body);
|
||||
}
|
||||
|
||||
// physical thrust axis is the negative of body z axis
|
||||
_thrust_sp = -_v_att_sp.thrust_body[2];
|
||||
|
||||
_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
|
||||
}
|
||||
|
||||
/*
|
||||
* Attitude rates controller.
|
||||
* Input: '_rates_sp' vector, '_thrust_sp'
|
||||
* Output: '_att_control' vector
|
||||
*/
|
||||
void
|
||||
MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates)
|
||||
{
|
||||
// reset integral if disarmed
|
||||
if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_rate_control.resetIntegral();
|
||||
}
|
||||
|
||||
const bool landed = _vehicle_land_detected.maybe_landed || _vehicle_land_detected.landed;
|
||||
_rate_control.setSaturationStatus(_saturation_status);
|
||||
_att_control = _rate_control.update(rates, _rates_sp, dt, landed);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::publish_rates_setpoint()
|
||||
{
|
||||
_v_rates_sp.roll = _rates_sp(0);
|
||||
_v_rates_sp.pitch = _rates_sp(1);
|
||||
_v_rates_sp.yaw = _rates_sp(2);
|
||||
_v_rates_sp.thrust_body[0] = 0.0f;
|
||||
_v_rates_sp.thrust_body[1] = 0.0f;
|
||||
_v_rates_sp.thrust_body[2] = -_thrust_sp;
|
||||
_v_rates_sp.timestamp = hrt_absolute_time();
|
||||
vehicle_rates_setpoint_s v_rates_sp{};
|
||||
|
||||
_v_rates_sp_pub.publish(_v_rates_sp);
|
||||
}
|
||||
v_rates_sp.roll = _rates_sp(0);
|
||||
v_rates_sp.pitch = _rates_sp(1);
|
||||
v_rates_sp.yaw = _rates_sp(2);
|
||||
v_rates_sp.thrust_body[0] = _v_att_sp.thrust_body[0];
|
||||
v_rates_sp.thrust_body[1] = _v_att_sp.thrust_body[1];
|
||||
v_rates_sp.thrust_body[2] = _v_att_sp.thrust_body[2];
|
||||
v_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::publish_rate_controller_status()
|
||||
{
|
||||
rate_ctrl_status_s rate_ctrl_status = {};
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
_rate_control.getRateControlStatus(rate_ctrl_status);
|
||||
_controller_status_pub.publish(rate_ctrl_status);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::publish_actuator_controls()
|
||||
{
|
||||
_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
|
||||
_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
|
||||
_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
|
||||
_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
|
||||
_actuators.control[7] = (float)_landing_gear.landing_gear;
|
||||
// note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run()
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
|
||||
/* scale effort by battery status */
|
||||
if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
_actuators.control[i] *= _battery_status.scale;
|
||||
}
|
||||
}
|
||||
|
||||
if (!_actuators_0_circuit_breaker_enabled) {
|
||||
orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
_v_rates_sp_pub.publish(v_rates_sp);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_angular_velocity_sub.unregisterCallback();
|
||||
_vehicle_attitude_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* run controller on gyro changes */
|
||||
vehicle_angular_velocity_s angular_velocity;
|
||||
// Check if parameters have changed
|
||||
if (_params_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_params_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
// run controller on attitude updates
|
||||
const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
|
||||
|
||||
if (_vehicle_attitude_sub.update(&_v_att)) {
|
||||
|
||||
// Check for a heading reset
|
||||
if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
|
||||
// we only extract the heading change from the delta quaternion
|
||||
_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
|
||||
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
|
||||
_last_run = now;
|
||||
|
||||
const Vector3f rates{angular_velocity.xyz};
|
||||
|
||||
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
|
||||
/* run the rate controller immediately after a gyro update */
|
||||
if (_v_control_mode.flag_control_rates_enabled) {
|
||||
control_attitude_rates(dt, rates);
|
||||
|
||||
publish_actuator_controls();
|
||||
publish_rate_controller_status();
|
||||
}
|
||||
|
||||
/* check for updates in other topics */
|
||||
_manual_control_sp_sub.update(&_manual_control_sp);
|
||||
_v_control_mode_sub.update(&_v_control_mode);
|
||||
_battery_status_sub.update(&_battery_status);
|
||||
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
|
||||
_landing_gear_sub.update(&_landing_gear);
|
||||
vehicle_status_poll();
|
||||
vehicle_motor_limits_poll();
|
||||
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
|
||||
const bool attitude_updated = vehicle_attitude_poll();
|
||||
|
||||
_attitude_dt += dt;
|
||||
|
||||
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
|
||||
* or roll (yaw can rotate 360 in normal att control). If both are true don't
|
||||
* even bother running the attitude controllers */
|
||||
* or roll (yaw can rotate 360 in normal att control). If both are true don't
|
||||
* even bother running the attitude controllers */
|
||||
if (_v_control_mode.flag_control_rattitude_enabled) {
|
||||
_v_control_mode.flag_control_attitude_enabled =
|
||||
fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() &&
|
||||
|
|
@ -490,90 +336,35 @@ MulticopterAttitudeControl::Run()
|
|||
|
||||
bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
|
||||
|
||||
|
||||
if (run_att_ctrl) {
|
||||
if (attitude_updated) {
|
||||
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
|
||||
if (_v_control_mode.flag_control_manual_enabled &&
|
||||
!_v_control_mode.flag_control_altitude_enabled &&
|
||||
!_v_control_mode.flag_control_velocity_enabled &&
|
||||
!_v_control_mode.flag_control_position_enabled) {
|
||||
generate_attitude_setpoint(_attitude_dt, _reset_yaw_sp);
|
||||
attitude_setpoint_generated = true;
|
||||
}
|
||||
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
|
||||
if (_v_control_mode.flag_control_manual_enabled &&
|
||||
!_v_control_mode.flag_control_altitude_enabled &&
|
||||
!_v_control_mode.flag_control_velocity_enabled &&
|
||||
!_v_control_mode.flag_control_position_enabled) {
|
||||
|
||||
control_attitude();
|
||||
|
||||
if (_v_control_mode.flag_control_yawrate_override_enabled) {
|
||||
/* Yaw rate override enabled, overwrite the yaw setpoint */
|
||||
_v_rates_sp_sub.update(&_v_rates_sp);
|
||||
const auto yawrate_reference = _v_rates_sp.yaw;
|
||||
_rates_sp(2) = yawrate_reference;
|
||||
}
|
||||
|
||||
publish_rates_setpoint();
|
||||
generate_attitude_setpoint(dt, _reset_yaw_sp);
|
||||
attitude_setpoint_generated = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* attitude controller disabled, poll rates setpoint topic */
|
||||
if (_v_control_mode.flag_control_manual_enabled && is_hovering) {
|
||||
if (manual_control_updated) {
|
||||
/* manual rates control - ACRO mode */
|
||||
Vector3f man_rate_sp(
|
||||
math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
|
||||
math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
|
||||
math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get()));
|
||||
_rates_sp = man_rate_sp.emult(_acro_rate_max);
|
||||
_thrust_sp = _manual_control_sp.z;
|
||||
publish_rates_setpoint();
|
||||
}
|
||||
control_attitude();
|
||||
|
||||
} else {
|
||||
/* attitude controller disabled, poll rates setpoint topic */
|
||||
if (_v_rates_sp_sub.update(&_v_rates_sp)) {
|
||||
_rates_sp(0) = _v_rates_sp.roll;
|
||||
_rates_sp(1) = _v_rates_sp.pitch;
|
||||
_rates_sp(2) = _v_rates_sp.yaw;
|
||||
_thrust_sp = -_v_rates_sp.thrust_body[2];
|
||||
}
|
||||
if (_v_control_mode.flag_control_yawrate_override_enabled) {
|
||||
/* Yaw rate override enabled, overwrite the yaw setpoint */
|
||||
_v_rates_sp_sub.update(&_v_rates_sp);
|
||||
const auto yawrate_reference = _v_rates_sp.yaw;
|
||||
_rates_sp(2) = yawrate_reference;
|
||||
}
|
||||
|
||||
publish_rates_setpoint();
|
||||
}
|
||||
|
||||
if (_v_control_mode.flag_control_termination_enabled) {
|
||||
if (!_vehicle_status.is_vtol) {
|
||||
_rates_sp.zero();
|
||||
_rate_control.resetIntegral();
|
||||
_thrust_sp = 0.0f;
|
||||
_att_control.zero();
|
||||
publish_actuator_controls();
|
||||
}
|
||||
}
|
||||
// reset yaw setpoint during transitions, tailsitter.cpp generates
|
||||
// attitude setpoint for the transition
|
||||
_reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
|
||||
_vehicle_land_detected.landed ||
|
||||
(_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);
|
||||
|
||||
if (attitude_updated) {
|
||||
// reset yaw setpoint during transitions, tailsitter.cpp generates
|
||||
// attitude setpoint for the transition
|
||||
_reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
|
||||
_vehicle_land_detected.landed ||
|
||||
(_vehicle_status.is_vtol && _vehicle_status.in_transition_mode);
|
||||
|
||||
_attitude_dt = 0.f;
|
||||
}
|
||||
|
||||
/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
|
||||
if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) {
|
||||
_dt_accumulator += dt;
|
||||
++_loop_counter;
|
||||
|
||||
if (_dt_accumulator > 1.f) {
|
||||
const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
|
||||
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
|
||||
_dt_accumulator = 0;
|
||||
_loop_counter = 0;
|
||||
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
|
||||
}
|
||||
}
|
||||
|
||||
parameter_update_poll();
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
|
|
@ -608,8 +399,6 @@ int MulticopterAttitudeControl::print_status()
|
|||
|
||||
perf_print_counter(_loop_perf);
|
||||
|
||||
print_message(_actuators);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -627,11 +416,10 @@ int MulticopterAttitudeControl::print_usage(const char *reason)
|
|||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements the multicopter attitude and rate controller. It takes attitude
|
||||
setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
|
||||
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
|
||||
This implements the multicopter attitude controller. It takes attitude
|
||||
setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint.
|
||||
|
||||
The controller has two loops: a P loop for angular error and a PID loop for angular rate error.
|
||||
The controller has a P loop for angular error
|
||||
|
||||
Publication documenting the implemented Quaternion Attitude Control:
|
||||
Nonlinear Quadrocopter Attitude Control (2013)
|
||||
|
|
@ -640,9 +428,6 @@ Institute for Dynamic Systems and Control (IDSC), ETH Zurich
|
|||
|
||||
https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
|
||||
|
||||
### Implementation
|
||||
To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("mc_att_control", "controller");
|
||||
|
|
|
|||
|
|
@ -53,87 +53,6 @@
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f);
|
||||
|
||||
/**
|
||||
* Roll rate P gain
|
||||
*
|
||||
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.5
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Roll rate I gain
|
||||
*
|
||||
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Roll rate integrator limit
|
||||
*
|
||||
* Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Roll rate D gain
|
||||
*
|
||||
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.01
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Roll rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Roll rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error
|
||||
* + MC_ROLLRATE_I * error_integral
|
||||
* + MC_ROLLRATE_D * error_derivative)
|
||||
* Set MC_ROLLRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_ROLLRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Pitch P gain
|
||||
*
|
||||
|
|
@ -148,86 +67,6 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
|
||||
|
||||
/**
|
||||
* Pitch rate P gain
|
||||
*
|
||||
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.6
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Pitch rate I gain
|
||||
*
|
||||
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Pitch rate integrator limit
|
||||
*
|
||||
* Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Pitch rate D gain
|
||||
*
|
||||
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Pitch rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error
|
||||
* + MC_PITCHRATE_I * error_integral
|
||||
* + MC_PITCHRATE_D * error_derivative)
|
||||
* Set MC_PITCHRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_PITCHRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Yaw P gain
|
||||
*
|
||||
|
|
@ -242,87 +81,6 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f);
|
||||
|
||||
/**
|
||||
* Yaw rate P gain
|
||||
*
|
||||
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.6
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f);
|
||||
|
||||
/**
|
||||
* Yaw rate I gain
|
||||
*
|
||||
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f);
|
||||
|
||||
/**
|
||||
* Yaw rate integrator limit
|
||||
*
|
||||
* Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Yaw rate D gain
|
||||
*
|
||||
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_YAWRATE_K * (MC_YAWRATE_P * error
|
||||
* + MC_YAWRATE_I * error_integral
|
||||
* + MC_YAWRATE_D * error_derivative)
|
||||
* Set MC_YAWRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_YAWRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Max roll rate
|
||||
*
|
||||
|
|
@ -373,107 +131,6 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f);
|
||||
|
||||
/**
|
||||
* Max acro roll rate
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro pitch rate
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro yaw rate
|
||||
* default 1.5 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Roll and Pitch.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Yaw.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Roll and Pitch.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 resonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Yaw.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 resonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
|
||||
|
||||
/**
|
||||
* Threshold for Rattitude mode
|
||||
*
|
||||
|
|
@ -487,35 +144,3 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
|
|||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f);
|
||||
|
||||
/**
|
||||
* Battery power level scaler
|
||||
*
|
||||
* This compensates for voltage drop of the battery over time by attempting to
|
||||
* normalize performance across the operating range of the battery. The copter
|
||||
* should constantly behave as if it was fully charged with reduced max acceleration
|
||||
* at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
|
||||
* it will still be 0.5 at 60% battery.
|
||||
*
|
||||
* @boolean
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
|
||||
|
||||
/**
|
||||
* Cutoff frequency for the low pass filter on the D-term in the rate controller
|
||||
*
|
||||
* The D-term uses the derivative of the rate and thus is the most susceptible to noise.
|
||||
* Therefore, using a D-term filter allows to decrease the driver-level filtering, which
|
||||
* leads to reduced control latency and permits to increase the P gains.
|
||||
* A value of 0 disables the filter.
|
||||
*
|
||||
* @unit Hz
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @decimal 0
|
||||
* @increment 10
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,48 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(RateControl)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__mc_rate_control
|
||||
MAIN mc_rate_control
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
MulticopterRateControl.cpp
|
||||
MulticopterRateControl.hpp
|
||||
DEPENDS
|
||||
circuit_breaker
|
||||
mathlib
|
||||
RateControl
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
@ -0,0 +1,412 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "MulticopterRateControl.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
using math::radians;
|
||||
|
||||
MulticopterRateControl::MulticopterRateControl() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
MulticopterRateControl::~MulticopterRateControl()
|
||||
{
|
||||
perf_free(_loop_perf);
|
||||
}
|
||||
|
||||
bool
|
||||
MulticopterRateControl::init()
|
||||
{
|
||||
if (!_vehicle_angular_velocity_sub.registerCallback()) {
|
||||
PX4_ERR("vehicle_angular_velocity callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterRateControl::parameters_updated()
|
||||
{
|
||||
// rate control parameters
|
||||
// The controller gain K is used to convert the parallel (P + I/s + sD) form
|
||||
// to the ideal (K * [1 + 1/sTi + sTd]) form
|
||||
const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
|
||||
|
||||
_rate_control.setGains(
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())),
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())),
|
||||
rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get())));
|
||||
|
||||
_rate_control.setIntegratorLimit(
|
||||
Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()));
|
||||
|
||||
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false);
|
||||
|
||||
_rate_control.setFeedForwardGain(
|
||||
Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
|
||||
|
||||
|
||||
// manual rate control acro mode rate limits
|
||||
_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
|
||||
radians(_param_mc_acro_y_max.get()));
|
||||
|
||||
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterRateControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (_actuators_id == nullptr) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
|
||||
|
||||
} else {
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
MulticopterRateControl::get_landing_gear_state()
|
||||
{
|
||||
// Only switch the landing gear up if we are not landed and if
|
||||
// the user switched from gear down to gear up.
|
||||
// If the user had the switch in the gear up position and took off ignore it
|
||||
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
|
||||
if (_landed) {
|
||||
_gear_state_initialized = false;
|
||||
}
|
||||
|
||||
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
|
||||
|
||||
if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
|
||||
landing_gear = landing_gear_s::GEAR_UP;
|
||||
|
||||
} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
|
||||
// Switching the gear off does put it into a safe defined state
|
||||
_gear_state_initialized = true;
|
||||
}
|
||||
|
||||
return landing_gear;
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterRateControl::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
_vehicle_angular_velocity_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
// Check if parameters have changed
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
parameters_updated();
|
||||
}
|
||||
|
||||
/* run controller on gyro changes */
|
||||
vehicle_angular_velocity_s angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
|
||||
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
|
||||
_last_run = now;
|
||||
|
||||
const Vector3f rates{angular_velocity.xyz};
|
||||
|
||||
/* check for updates in other topics */
|
||||
_v_control_mode_sub.update(&_v_control_mode);
|
||||
|
||||
if (_vehicle_land_detected_sub.updated()) {
|
||||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
_landed = vehicle_land_detected.landed;
|
||||
_maybe_landed = vehicle_land_detected.maybe_landed;
|
||||
}
|
||||
}
|
||||
|
||||
vehicle_status_poll();
|
||||
|
||||
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
|
||||
|
||||
// generate the rate setpoint from sticks?
|
||||
bool manual_rate_sp = false;
|
||||
|
||||
if (_v_control_mode.flag_control_manual_enabled &&
|
||||
!_v_control_mode.flag_control_altitude_enabled &&
|
||||
!_v_control_mode.flag_control_velocity_enabled &&
|
||||
!_v_control_mode.flag_control_position_enabled) {
|
||||
|
||||
// landing gear controlled from stick inputs if we are in Manual/Stabilized mode
|
||||
// limit landing gear update rate to 50 Hz
|
||||
if (hrt_elapsed_time(&_landing_gear.timestamp) > 20_ms) {
|
||||
_landing_gear.landing_gear = get_landing_gear_state();
|
||||
_landing_gear.timestamp = hrt_absolute_time();
|
||||
_landing_gear_pub.publish(_landing_gear);
|
||||
}
|
||||
|
||||
if (!_v_control_mode.flag_control_attitude_enabled) {
|
||||
manual_rate_sp = true;
|
||||
}
|
||||
|
||||
// Check if we are in rattitude mode and the pilot is within the center threshold on pitch and roll
|
||||
// if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro)
|
||||
if (_v_control_mode.flag_control_rattitude_enabled) {
|
||||
manual_rate_sp =
|
||||
(fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) ||
|
||||
(fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get());
|
||||
}
|
||||
|
||||
} else {
|
||||
_landing_gear_sub.update(&_landing_gear);
|
||||
}
|
||||
|
||||
if (manual_rate_sp) {
|
||||
if (manual_control_updated) {
|
||||
|
||||
// manual rates control - ACRO mode
|
||||
const Vector3f man_rate_sp{
|
||||
math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
|
||||
math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
|
||||
math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
|
||||
|
||||
_rates_sp = man_rate_sp.emult(_acro_rate_max);
|
||||
_thrust_sp = _manual_control_sp.z;
|
||||
|
||||
// publish rate setpoint
|
||||
vehicle_rates_setpoint_s v_rates_sp{};
|
||||
v_rates_sp.roll = _rates_sp(0);
|
||||
v_rates_sp.pitch = _rates_sp(1);
|
||||
v_rates_sp.yaw = _rates_sp(2);
|
||||
v_rates_sp.thrust_body[0] = 0.0f;
|
||||
v_rates_sp.thrust_body[1] = 0.0f;
|
||||
v_rates_sp.thrust_body[2] = -_thrust_sp;
|
||||
v_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
_v_rates_sp_pub.publish(v_rates_sp);
|
||||
}
|
||||
|
||||
} else {
|
||||
// use rates setpoint topic
|
||||
vehicle_rates_setpoint_s v_rates_sp;
|
||||
|
||||
if (_v_rates_sp_sub.update(&v_rates_sp)) {
|
||||
_rates_sp(0) = v_rates_sp.roll;
|
||||
_rates_sp(1) = v_rates_sp.pitch;
|
||||
_rates_sp(2) = v_rates_sp.yaw;
|
||||
_thrust_sp = -v_rates_sp.thrust_body[2];
|
||||
}
|
||||
}
|
||||
|
||||
// calculate loop update rate while disarmed or at least a few times (updating the filter is expensive)
|
||||
if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) {
|
||||
_dt_accumulator += dt;
|
||||
++_loop_counter;
|
||||
|
||||
if (_dt_accumulator > 1.0f) {
|
||||
const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
|
||||
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
|
||||
_dt_accumulator = 0;
|
||||
_loop_counter = 0;
|
||||
_rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true);
|
||||
}
|
||||
}
|
||||
|
||||
// run the rate controller
|
||||
if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
|
||||
|
||||
// reset integral if disarmed
|
||||
if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_rate_control.resetIntegral();
|
||||
}
|
||||
|
||||
// update saturation status from mixer feedback
|
||||
if (_motor_limits_sub.updated()) {
|
||||
multirotor_motor_limits_s motor_limits;
|
||||
|
||||
if (_motor_limits_sub.copy(&motor_limits)) {
|
||||
MultirotorMixer::saturation_status saturation_status;
|
||||
saturation_status.value = motor_limits.saturation_status;
|
||||
|
||||
_rate_control.setSaturationStatus(saturation_status);
|
||||
}
|
||||
}
|
||||
|
||||
// run rate controller
|
||||
const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed);
|
||||
|
||||
// publish rate controller status
|
||||
rate_ctrl_status_s rate_ctrl_status{};
|
||||
_rate_control.getRateControlStatus(rate_ctrl_status);
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
_controller_status_pub.publish(rate_ctrl_status);
|
||||
|
||||
// publish actuator controls
|
||||
actuator_controls_s actuators{};
|
||||
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
|
||||
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
|
||||
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
|
||||
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
|
||||
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
|
||||
actuators.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
|
||||
// scale effort by battery status if enabled
|
||||
if (_param_mc_bat_scale_en.get()) {
|
||||
if (_battery_status_sub.updated()) {
|
||||
battery_status_s battery_status;
|
||||
|
||||
if (_battery_status_sub.copy(&battery_status)) {
|
||||
_battery_status_scale = battery_status.scale;
|
||||
}
|
||||
}
|
||||
|
||||
if (_battery_status_scale > 0.0f) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
actuators.control[i] *= _battery_status_scale;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
|
||||
|
||||
} else if (_v_control_mode.flag_control_termination_enabled) {
|
||||
if (!_vehicle_status.is_vtol) {
|
||||
// publish actuator controls
|
||||
actuator_controls_s actuators{};
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
int MulticopterRateControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MulticopterRateControl *instance = new MulticopterRateControl();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int MulticopterRateControl::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
perf_print_counter(_loop_perf);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int MulticopterRateControl::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int MulticopterRateControl::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements the multicopter rate controller. It takes rate setpoints (in acro mode
|
||||
via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
|
||||
|
||||
The controller has a PID loop for angular rate error.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Multicopter rate control app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
|
||||
|
||||
int mc_rate_control_main(int argc, char *argv[])
|
||||
{
|
||||
return MulticopterRateControl::main(argc, argv);
|
||||
}
|
||||
|
|
@ -0,0 +1,194 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <RateControl.hpp>
|
||||
|
||||
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
MulticopterRateControl();
|
||||
|
||||
virtual ~MulticopterRateControl();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
void Run() override;
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* initialize some vectors/matrices from parameters
|
||||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Get the landing gear state based on the manual control switch position
|
||||
* @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
|
||||
*/
|
||||
float get_landing_gear_state();
|
||||
|
||||
RateControl _rate_control; ///< class for rate control calculations
|
||||
|
||||
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
|
||||
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
|
||||
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
|
||||
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
|
||||
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
|
||||
|
||||
orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */
|
||||
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
|
||||
|
||||
landing_gear_s _landing_gear{};
|
||||
manual_control_setpoint_s _manual_control_sp{};
|
||||
vehicle_control_mode_s _v_control_mode{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
|
||||
bool _landed{true};
|
||||
bool _maybe_landed{true};
|
||||
|
||||
float _battery_status_scale{0.0f};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop duration performance counter */
|
||||
|
||||
static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */
|
||||
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
|
||||
|
||||
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
|
||||
|
||||
float _thrust_sp{0.0f}; /**< thrust setpoint */
|
||||
|
||||
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
|
||||
|
||||
hrt_abstime _task_start{hrt_absolute_time()};
|
||||
hrt_abstime _last_run{0};
|
||||
float _dt_accumulator{0.0f};
|
||||
int _loop_counter{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
|
||||
(ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_K>) _param_mc_rollrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
|
||||
(ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
|
||||
(ParamFloat<px4::params::MC_PITCHRATE_K>) _param_mc_pitchrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
|
||||
(ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
|
||||
(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
|
||||
|
||||
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
|
||||
|
||||
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
|
||||
|
||||
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
|
||||
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
|
||||
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
|
||||
|
||||
(ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
|
||||
|
||||
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
|
||||
|
||||
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
|
||||
)
|
||||
|
||||
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -72,7 +72,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
|
|||
Vector3f rate_error = rate_sp - rate;
|
||||
|
||||
// prepare D-term based on low-pass filtered rates
|
||||
Vector3f rate_filtered(_lp_filters_d.apply(rate));
|
||||
const Vector3f rate_filtered(_lp_filters_d.apply(rate));
|
||||
Vector3f rate_d;
|
||||
|
||||
if (dt > FLT_EPSILON) {
|
||||
|
|
@ -80,7 +80,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
|
|||
}
|
||||
|
||||
// PID control with feed forward
|
||||
Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
|
||||
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
|
||||
|
||||
_rate_prev = rate;
|
||||
_rate_prev_filtered = rate_filtered;
|
||||
|
|
@ -0,0 +1,415 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mc_rate_control_params.c
|
||||
* Parameters for multicopter attitude controller.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Roll rate P gain
|
||||
*
|
||||
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.5
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Roll rate I gain
|
||||
*
|
||||
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Roll rate integrator limit
|
||||
*
|
||||
* Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Roll rate D gain
|
||||
*
|
||||
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.01
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Roll rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Roll rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error
|
||||
* + MC_ROLLRATE_I * error_integral
|
||||
* + MC_ROLLRATE_D * error_derivative)
|
||||
* Set MC_ROLLRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_ROLLRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate P gain
|
||||
*
|
||||
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.6
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Pitch rate I gain
|
||||
*
|
||||
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Pitch rate integrator limit
|
||||
*
|
||||
* Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Pitch rate D gain
|
||||
*
|
||||
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Pitch rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error
|
||||
* + MC_PITCHRATE_I * error_integral
|
||||
* + MC_PITCHRATE_D * error_derivative)
|
||||
* Set MC_PITCHRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_PITCHRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate P gain
|
||||
*
|
||||
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.6
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f);
|
||||
|
||||
/**
|
||||
* Yaw rate I gain
|
||||
*
|
||||
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f);
|
||||
|
||||
/**
|
||||
* Yaw rate integrator limit
|
||||
*
|
||||
* Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Yaw rate D gain
|
||||
*
|
||||
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = MC_YAWRATE_K * (MC_YAWRATE_P * error
|
||||
* + MC_YAWRATE_I * error_integral
|
||||
* + MC_YAWRATE_D * error_derivative)
|
||||
* Set MC_YAWRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set MC_YAWRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Max acro roll rate
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro pitch rate
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro yaw rate
|
||||
* default 1.5 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Roll and Pitch.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Yaw.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Roll and Pitch.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 resonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Yaw.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 resonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
|
||||
|
||||
/**
|
||||
* Battery power level scaler
|
||||
*
|
||||
* This compensates for voltage drop of the battery over time by attempting to
|
||||
* normalize performance across the operating range of the battery. The copter
|
||||
* should constantly behave as if it was fully charged with reduced max acceleration
|
||||
* at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
|
||||
* it will still be 0.5 at 60% battery.
|
||||
*
|
||||
* @boolean
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
|
||||
|
||||
/**
|
||||
* Cutoff frequency for the low pass filter on the D-term in the rate controller
|
||||
*
|
||||
* The D-term uses the derivative of the rate and thus is the most susceptible to noise.
|
||||
* Therefore, using a D-term filter allows to decrease the driver-level filtering, which
|
||||
* leads to reduced control latency and permits to increase the P gains.
|
||||
* A value of 0 disables the filter.
|
||||
*
|
||||
* @unit Hz
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @decimal 0
|
||||
* @increment 10
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f);
|
||||
|
||||
|
|
@ -183,7 +183,7 @@ protected:
|
|||
struct vtol_vehicle_status_s *_vtol_vehicle_status;
|
||||
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
|
||||
struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
|
||||
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control
|
||||
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_rate_control
|
||||
struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control
|
||||
struct vehicle_local_position_s *_local_pos;
|
||||
struct vehicle_local_position_setpoint_s *_local_pos_sp;
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ usage(const char *reason)
|
|||
"\n"
|
||||
"Calibration procedure (running the command will guide you through it):\n"
|
||||
"- Remove props, power off the ESC's\n"
|
||||
"- Stop attitude controllers: mc_att_control stop, fw_att_control stop\n"
|
||||
"- Stop attitude and rate controllers: mc_rate_control stop, fw_att_control stop\n"
|
||||
"- Make sure safety is off\n"
|
||||
"- Run this command\n"
|
||||
);
|
||||
|
|
@ -218,7 +218,7 @@ esc_calib_main(int argc, char *argv[])
|
|||
|
||||
if (orb_updated) {
|
||||
PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
|
||||
"\tmc_att_control stop\n"
|
||||
"\tmc_rate_control stop\n"
|
||||
"\tfw_att_control stop\n");
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -120,7 +120,7 @@ usage(const char *reason)
|
|||
Application to test motor ramp up.
|
||||
|
||||
Before starting, make sure to stop any running attitude controller:
|
||||
$ mc_att_control stop
|
||||
$ mc_rate_control stop
|
||||
$ fw_att_control stop
|
||||
|
||||
When starting, a background task is started, runs for several seconds (as specified), then exits.
|
||||
|
|
@ -327,7 +327,7 @@ int prepare(int fd, unsigned long *max_channels)
|
|||
|
||||
if (orb_updated) {
|
||||
PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
|
||||
"\tmc_att_control stop\n"
|
||||
"\tmc_rate_control stop\n"
|
||||
"\tfw_att_control stop\n");
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue