fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

initial control allocation support

Browse Source 
- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>
This commit is contained in:
Julien Lecoeur 2021-01-10 11:30:23 -05:00 committed by Daniel Agar
parent fc6b61dad1
commit 343cf5603e
68 changed files with 6129 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc Normal file
View File

@ -0,0 +1,52 @@
#!/bin/sh
#
# @name 3DR Iris Quadrotor SITL
#
# @type Quadrotor Wide
#
# @maintainer Julian Oes <julian@oes.ch>
#
sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc
if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0
param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05
param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_MC_R0_PX 0.1515
param set CA_MC_R0_PY 0.245
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM 0.05
param set CA_MC_R1_PX -0.1515
param set CA_MC_R1_PY -0.1875
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.1515
param set CA_MC_R2_PY -0.245
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.1515
param set CA_MC_R3_PY 0.1875
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM -0.05
fi
set MIXER direct

75
ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc Normal file
View File

@ -0,0 +1,75 @@
#!/bin/sh
#
# @name Typhoon H480 SITL
#
# @type Hexarotor x
#
sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc
if [ $AUTOCNF = yes ]
then
param set MPC_XY_VEL_I_ACC 4
param set MPC_XY_VEL_P_ACC 3
param set RTL_DESCEND_ALT 10
param set RTL_LAND_DELAY 0
param set TRIG_INTERFACE 3
param set TRIG_MODE 4
param set MNT_MODE_IN 0
param set MAV_PROTO_VER 2
param set MPC_USE_HTE 0
param set VM_MASS 2.66
param set VM_INERTIA_XX 0.06
param set VM_INERTIA_YY 0.06
param set VM_INERTIA_ZZ 0.10
param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT4_MIN 0.0
param set CA_ACT5_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_ACT4_MAX 1.0
param set CA_ACT5_MAX 1.0
param set CA_MC_R0_PX 0.0
param set CA_MC_R0_PY 1.0
param set CA_MC_R0_CT 9.5
param set CA_MC_R0_KM -0.05
param set CA_MC_R1_PX 0.0
param set CA_MC_R1_PY -1.0
param set CA_MC_R1_CT 9.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.866025
param set CA_MC_R2_PY -0.5
param set CA_MC_R2_CT 9.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.866025
param set CA_MC_R3_PY 0.5
param set CA_MC_R3_CT 9.5
param set CA_MC_R3_KM 0.05
param set CA_MC_R4_PX 0.866025
param set CA_MC_R4_PY 0.5
param set CA_MC_R4_CT 9.5
param set CA_MC_R4_KM 0.05
param set CA_MC_R5_PX -0.866025
param set CA_MC_R5_PY -0.5
param set CA_MC_R5_CT 9.5
param set CA_MC_R5_KM -0.05
fi
set MAV_TYPE 13
# set MIXER hexa_x
set MIXER direct

10
ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post Normal file
View File

@ -0,0 +1,10 @@
mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
# shellcheck disable=SC2154
mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
# shellcheck disable=SC2154
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local

1
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -54,4 +54,5 @@ px4_add_romfs_files(
rc.vehicle_setup
rc.vtol_apps
rc.vtol_defaults
rc.ctrlalloc
)

57
ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc Normal file
View File

@ -0,0 +1,57 @@
#!/bin/sh
#
# @name S500 with control allocation
#
# @type Quadrotor x
# @class Copter
#
# @maintainer Silvan Fuhrer
#
sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc
if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0
param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05
param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_MC_R0_PX 0.177
param set CA_MC_R0_PY 0.177
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM 0.05
param set CA_MC_R1_PX -0.177
param set CA_MC_R1_PY -0.177
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.177
param set CA_MC_R2_PY -0.177
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.177
param set CA_MC_R3_PY 0.177
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM -0.05
fi
set MIXER direct
set PWM_OUT 1234
set MIXER_AUX direct_aux
set PWM_AUX_OUT 1234

75
ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc Normal file
View File

@ -0,0 +1,75 @@
#!/bin/sh
#
# @name Hex X with control allocation
#
# @type Hexarotor x
# @class Copter
#
# @maintainer Silvan Fuhrer
#
sh /etc/init.d/rc.mc_defaults
sh /etc/init.d/rc.ctrlalloc
if [ $AUTOCNF = yes ]
then
param set MPC_USE_HTE 0
param set VM_MASS 1.5
param set VM_INERTIA_XX 0.03
param set VM_INERTIA_YY 0.03
param set VM_INERTIA_ZZ 0.05
param set CA_AIRFRAME 0
param set CA_METHOD 1
param set CA_ACT0_MIN 0.0
param set CA_ACT1_MIN 0.0
param set CA_ACT2_MIN 0.0
param set CA_ACT3_MIN 0.0
param set CA_ACT4_MIN 0.0
param set CA_ACT5_MIN 0.0
param set CA_ACT0_MAX 1.0
param set CA_ACT1_MAX 1.0
param set CA_ACT2_MAX 1.0
param set CA_ACT3_MAX 1.0
param set CA_ACT4_MAX 1.0
param set CA_ACT5_MAX 1.0
param set CA_MC_R0_PX 0.0
param set CA_MC_R0_PY 0.275
param set CA_MC_R0_CT 6.5
param set CA_MC_R0_KM -0.05
param set CA_MC_R1_PX 0.0
param set CA_MC_R1_PY -0.275
param set CA_MC_R1_CT 6.5
param set CA_MC_R1_KM 0.05
param set CA_MC_R2_PX 0.238
param set CA_MC_R2_PY -0.1375
param set CA_MC_R2_CT 6.5
param set CA_MC_R2_KM -0.05
param set CA_MC_R3_PX -0.238
param set CA_MC_R3_PY 0.1375
param set CA_MC_R3_CT 6.5
param set CA_MC_R3_KM 0.05
param set CA_MC_R4_PX 0.238
param set CA_MC_R4_PY 0.1375
param set CA_MC_R4_CT 6.5
param set CA_MC_R4_KM 0.05
param set CA_MC_R5_PX -0.238
param set CA_MC_R5_PY -0.1375
param set CA_MC_R5_CT 6.5
param set CA_MC_R5_KM -0.05
fi
set MIXER direct
set PWM_OUT 123456
set MIXER_AUX direct_aux
set PWM_AUX_OUT 123456

26
ROMFS/px4fmu_common/init.d/rc.ctrlalloc Normal file
View File

@ -0,0 +1,26 @@
#!/bin/sh
#
# Standard apps for new control allocation and controllers
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start angular velocity controller
#
angular_velocity_controller start
mc_rate_control stop
#
# Start Control Allocator
#
control_allocator start
#
# Disable hover thrust estimator and prearming
# These features are currently incompatible with control allocation
#
# TODO: fix
#
param set MPC_USE_HTE 0
param set COM_PREARM_MODE 0

1
ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
View File

@ -43,4 +43,5 @@ px4_add_romfs_files(
tiltrotor_sitl.main.mix
uuv_x_sitl.main.mix
vectored6dof_sitl.main.mix
tiltrotor_sitl_direct.main.mix
)

14
ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix Normal file
View File

@ -0,0 +1,14 @@
Mixer for quad tiltrotor (x motor configuration)
================================================
A: 0
A: 1
A: 2
A: 3
A: 4
A: 5
A: 6
A: 7
A: 8
A: 9
A: 10

1
ROMFS/px4fmu_common/mixers/CMakeLists.txt
View File

@ -47,6 +47,7 @@ px4_add_romfs_files(
coax.main.mix
delta.main.mix
deltaquad.main.mix
direct.main.mix
dodeca_bottom_cox.aux.mix
dodeca_top_cox.main.mix
firefly6.aux.mix

10
ROMFS/px4fmu_common/mixers/direct.main.mix Normal file
View File

@ -0,0 +1,10 @@
# Direct mixer
A: 0
A: 1
A: 2
A: 3
A: 4
A: 5
A: 6
A: 7

2
Tools/px_romfs_pruner.py
View File

@ -97,7 +97,7 @@ def main():
# handle mixer files differently than startup files
if file_path.endswith(".mix"):
if line.startswith(("Z:", "M:", "R: ", "O:", "S:",
"H:", "T:", "P:")):
"H:", "T:", "P:", "A:")):
# reduce multiple consecutive spaces into a
# single space
line_reduced = re.sub(' +', ' ', line)

134
boards/px4/fmu-v3/ctrlalloc.cmake Normal file
View File

@ -0,0 +1,134 @@
# FMUv3 is FMUv2 with access to the full 2MB flash
px4_add_board(
PLATFORM nuttx
VENDOR px4
MODEL fmu-v3
LABEL ctrlalloc
TOOLCHAIN arm-none-eabi
ARCHITECTURE cortex-m4
ROMFSROOT px4fmu_common
IO px4_io-v2_default
TESTING
UAVCAN_INTERFACES 2
SERIAL_PORTS
GPS1:/dev/ttyS3
TEL1:/dev/ttyS1
TEL2:/dev/ttyS2
TEL4:/dev/ttyS6
DRIVERS
adc
barometer # all available barometer drivers
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
dshot
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/adis16477
imu/adis16497
imu/l3gd20
imu/lsm303d
imu/invensense/icm20608g
imu/invensense/mpu6000
imu/invensense/mpu9250
imu/icm20948
irlock
lights/blinkm
lights/rgbled
lights/rgbled_ncp5623c
#lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd
pca9685
#power_monitor/ina226
#protocol_splitter
pwm_input
pwm_out_sim
pwm_out
px4io
roboclaw
tap_esc
telemetry # all available telemetry drivers
test_ppm
tone_alarm
uavcan
MODULES
airspeed_selector
angular_velocity_controller
attitude_estimator_q
battery_status
camera_feedback
commander
control_allocator
dataman
ekf2
esc_battery
events
fw_att_control
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
local_position_estimator
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
rc_update
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS
bl_update
#dmesg
dumpfile
esc_calib
gpio
hardfault_log
i2cdetect
led_control
mixer
motor_ramp
motor_test
mtd
nshterm
param
perf
pwm
reboot
reflect
sd_bench
tests # tests and test runner
top
topic_listener
tune_control
usb_connected
ver
work_queue
EXAMPLES
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
hello
hwtest # Hardware test
#matlab_csv_serial
px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
work_item
)

128
boards/px4/fmu-v4/ctrlalloc.cmake Normal file
View File

@ -0,0 +1,128 @@
px4_add_board(
PLATFORM nuttx
VENDOR px4
MODEL fmu-v4
LABEL default
TOOLCHAIN arm-none-eabi
ARCHITECTURE cortex-m4
ROMFSROOT px4fmu_common
TESTING
UAVCAN_INTERFACES 1
SERIAL_PORTS
GPS1:/dev/ttyS3
TEL1:/dev/ttyS1
TEL2:/dev/ttyS2
DRIVERS
adc
barometer # all available barometer drivers
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
dshot
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
imu/invensense/icm20608g
imu/invensense/icm40609d
imu/invensense/mpu6500
imu/invensense/mpu9250
irlock
lights/blinkm
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
#protocol_splitter
pwm_input
pwm_out_sim
pwm_out
rc_input
roboclaw
safety_button
tap_esc
telemetry # all available telemetry drivers
test_ppm
tone_alarm
uavcan
MODULES
airspeed_selector
angular_velocity_controller
attitude_estimator_q
battery_status
camera_feedback
commander
control_allocator
dataman
ekf2
esc_battery
events
fw_att_control
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
local_position_estimator
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
rc_update
rover_pos_control
sensors
sih
temperature_compensation
uuv_att_control
vmount
vtol_att_control
SYSTEMCMDS
bl_update
#dmesg
dumpfile
esc_calib
gpio
hardfault_log
i2cdetect
led_control
mixer
motor_ramp
motor_test
mtd
nshterm
param
perf
pwm
reboot
reflect
sd_bench
tests # tests and test runner
top
topic_listener
tune_control
usb_connected
ver
work_queue
EXAMPLES
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
hello
hwtest # Hardware test
#matlab_csv_serial
px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
work_item
)

132
boards/px4/fmu-v5/ctrlalloc.cmake Normal file
View File

@ -0,0 +1,132 @@
px4_add_board(
PLATFORM nuttx
VENDOR px4
MODEL fmu-v5
LABEL default
TOOLCHAIN arm-none-eabi
ARCHITECTURE cortex-m7
ROMFSROOT px4fmu_common
IO px4_io-v2_default
TESTING
UAVCAN_INTERFACES 2
SERIAL_PORTS
GPS1:/dev/ttyS0
TEL1:/dev/ttyS1
TEL2:/dev/ttyS2
TEL4:/dev/ttyS3
DRIVERS
adc
barometer # all available barometer drivers
batt_smbus
camera_capture
camera_trigger
differential_pressure # all available differential pressure drivers
distance_sensor # all available distance sensor drivers
dshot
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver
irlock
lights/blinkm
lights/rgbled
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
power_monitor/ina226
#protocol_splitter
pwm_input
pwm_out_sim
pwm_out
px4io
rc_input
roboclaw
rpm
safety_button
tap_esc
telemetry # all available telemetry drivers
test_ppm
tone_alarm
uavcan
MODULES
airspeed_selector
angular_velocity_controller
attitude_estimator_q
battery_status
camera_feedback
commander
control_allocator
dataman
ekf2
esc_battery
events
fw_att_control
fw_pos_control_l1
land_detector
landing_target_estimator
load_mon
local_position_estimator
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
#micrortps_bridge
navigator
rc_update
rover_pos_control
sensors
sih
temperature_compensation
vmount
vtol_att_control
SYSTEMCMDS
bl_update
dmesg
dumpfile
esc_calib
gpio
hardfault_log
i2cdetect
led_control
mixer
motor_ramp
motor_test
mtd
nshterm
param
perf
pwm
reboot
reflect
sd_bench
tests # tests and test runner
top
topic_listener
tune_control
usb_connected
ver
work_queue
EXAMPLES
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
hello
hwtest # Hardware test
#matlab_csv_serial
px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
work_item
)

108
boards/px4/sitl/ctrlalloc.cmake Normal file
View File

@ -0,0 +1,108 @@
px4_add_board(
PLATFORM posix
VENDOR px4
MODEL sitl
LABEL default
TESTING
DRIVERS
#barometer # all available barometer drivers
#batt_smbus
camera_capture
camera_trigger
#differential_pressure # all available differential pressure drivers
#distance_sensor # all available distance sensor drivers
gps
#imu # all available imu drivers
#magnetometer # all available magnetometer drivers
pwm_out_sim
rpm/rpm_simulator
#telemetry # all available telemetry drivers
tone_alarm
#uavcan
MODULES
airspeed_selector
angular_velocity_controller
attitude_estimator_q
camera_feedback
commander
control_allocator
dataman
ekf2
events
fw_att_control
fw_pos_control_l1
land_detector
landing_target_estimator
#load_mon
local_position_estimator
logger
mavlink
mc_att_control
mc_hover_thrust_estimator
mc_pos_control
mc_rate_control
navigator
rc_update
replay
rover_pos_control
sensors
#sih
simulator
temperature_compensation
vmount
vtol_att_control
uuv_att_control
SYSTEMCMDS
#dumpfile
dyn
esc_calib
led_control
mixer
motor_ramp
motor_test
#mtd
#nshterm
param
perf
pwm
sd_bench
shutdown
tests # tests and test runner
#top
topic_listener
tune_control
ver
work_queue
EXAMPLES
dyn_hello # dynamically loading modules example
fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
hello
#hwtest # Hardware test
#matlab_csv_serial
px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
rover_steering_control # Rover example app
uuv_example_app
work_item
)
set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl")
set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none")
set(config_sitl_debugger disable CACHE STRING "debugger for sitl")
set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb")
# If the environment variable 'replay' is defined, we are building with replay
# support. In this case, we enable the orb publisher rules.
set(REPLAY_FILE "$ENV{replay}")
if(REPLAY_FILE)
message(STATUS "Building with uorb publisher rules support")
add_definitions(-DORB_USE_PUBLISHER_RULES)
message(STATUS "Building without lockstep for replay")
set(ENABLE_LOCKSTEP_SCHEDULER no)
else()
set(ENABLE_LOCKSTEP_SCHEDULER yes)
endif()

1
boards/px4/sitl/test.cmake
View File

@ -28,6 +28,7 @@ px4_add_board(
attitude_estimator_q
camera_feedback
commander
control_allocator
dataman
ekf2
events

5
msg/CMakeLists.txt
View File

@ -48,6 +48,7 @@ set(msg_files
collision_constraints.msg
collision_report.msg
commander_state.msg
control_allocator_status.msg
cpuload.msg
differential_pressure.msg
distance_sensor.msg
@ -140,8 +141,10 @@ set(msg_files
ulog_stream.msg
ulog_stream_ack.msg
vehicle_acceleration.msg
vehicle_actuator_setpoint.msg
vehicle_air_data.msg
vehicle_angular_acceleration.msg
vehicle_angular_acceleration_setpoint.msg
vehicle_angular_velocity.msg
vehicle_attitude.msg
vehicle_attitude_setpoint.msg
@ -162,6 +165,8 @@ set(msg_files
vehicle_roi.msg
vehicle_status.msg
vehicle_status_flags.msg
vehicle_thrust_setpoint.msg
vehicle_torque_setpoint.msg
vehicle_trajectory_bezier.msg
vehicle_trajectory_waypoint.msg
vtol_vehicle_status.msg

5
msg/actuator_controls.msg
View File

@ -1,6 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint8 NUM_ACTUATOR_CONTROLS = 8
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6
uint8 INDEX_ROLL = 0
uint8 INDEX_PITCH = 1
uint8 INDEX_YAW = 2
@ -16,10 +16,13 @@ uint8 GROUP_INDEX_ATTITUDE = 0
uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
uint8 GROUP_INDEX_GIMBAL = 2
uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
uint8 GROUP_INDEX_ALLOCATED_PART1 = 4
uint8 GROUP_INDEX_ALLOCATED_PART2 = 5
uint8 GROUP_INDEX_PAYLOAD = 6
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
float32[8] control
# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
# TOPICS actuator_controls_4 actuator_controls_5
# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

21
msg/control_allocator_status.msg Normal file
View File

@ -0,0 +1,21 @@
uint64 timestamp # time since system start (microseconds)
uint8 torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
float32[3] allocated_torque # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_torque = torque_setpoint - allocated_torque
uint8 thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
float32[3] allocated_thrust # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved.
# Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
int8 ACTUATOR_SATURATION_OK = 0 # The actuator is not saturated
int8 ACTUATOR_SATURATION_UPPER_DYN = 1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster
int8 ACTUATOR_SATURATION_UPPER = 2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value
int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster
int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value
int8[16] actuator_saturation # Indicates actuator saturation status.
# Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved.
# Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value.

10
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -298,6 +298,16 @@ rtps:
id: 141
- msg: rtl_flight_time
id: 142
- msg: vehicle_angular_acceleration_setpoint
id: 143
- msg: vehicle_torque_setpoint
id: 144
- msg: vehicle_thrust_setpoint
id: 145
- msg: vehicle_actuator_setpoint
id: 146
- msg: control_allocator_status
id: 147
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 170

6
msg/vehicle_actuator_setpoint.msg Normal file
View File

@ -0,0 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp the data this control response is based on was sampled
uint8 NUM_ACTUATOR_SETPOINT = 16
float32[16] actuator

5
msg/vehicle_angular_acceleration_setpoint.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2

5
msg/vehicle_thrust_setpoint.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # thrust setpoint along X, Y, Z body axis (in N)

5
msg/vehicle_torque_setpoint.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
float32[3] xyz # torque setpoint about X, Y, Z body axis (in N.m)

1
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -49,6 +49,7 @@ struct wq_config_t {
namespace wq_configurations
{
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority
static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl
static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1};
static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2};

2
platforms/posix/cmake/sitl_target.cmake
View File

@ -92,7 +92,7 @@ ExternalProject_Add(jsbsim_bridge
set(viewers none jmavsim gazebo)
set(debuggers none ide gdb lldb ddd valgrind callgrind)
set(models none shell
if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480
if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_ctrlalloc iris_obs_avoid iris_rtps px4vision solo typhoon_h480
plane plane_cam plane_catapult plane_lidar techpod
standard_vtol tailsitter tiltrotor
rover r1_rover boat cloudship

1
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp
View File

@ -43,6 +43,7 @@ namespace math
void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1.zero();

4
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp
View File

@ -74,12 +74,16 @@ public:
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
// Reset the filter state to this value
matrix::Vector3f reset(const matrix::Vector3f &sample);
private:
float _cutoff_freq{0.0f};
float _sample_freq{0.0f};
float _a1{0.0f};
float _a2{0.0f};

149
src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp Normal file
View File

@ -0,0 +1,149 @@
/****************************************************************************
*
* Copyright (c) 2020 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 mixer_AllocatedActuatorMixer.cpp
*
* Mixer for allocated actuators.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "AllocatedActuatorMixer.hpp"
#include <mathlib/mathlib.h>
#include <cstdio>
#include <px4_platform_common/defines.h>
// #define debug(fmt, args...) do { } while(0)
#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb,
uintptr_t cb_handle,
uint8_t index) :
Mixer(control_cb, cb_handle)
{
if (index < 8) {
_control_group = 4;
_control_index = index;
} else if (index < 16) {
_control_group = 5;
_control_index = index - 8;
} else {
debug("'A:' invalid index");
}
}
unsigned AllocatedActuatorMixer::set_trim(float trim)
{
return 1;
}
unsigned AllocatedActuatorMixer::get_trim(float *trim)
{
*trim = 0.0f;
return 1;
}
int
AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index)
{
int ret;
int i;
// enforce that the mixer ends with a new line
if (!string_well_formed(buf, buflen)) {
return -1;
}
// parse line
if ((ret = sscanf(buf, "A: %d", &i)) != 1) {
debug("'A:' parser: failed on '%s'", buf);
return -1;
}
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("'A:' parser: no line ending, line is incomplete");
return -1;
}
// check parsed index
if (i < 16) {
index = i;
} else {
debug("'A:' parser: invalid index");
return -1;
}
return 0;
}
AllocatedActuatorMixer *
AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen)
{
uint8_t index;
if (parse(buf, buflen, index) == 0) {
return new AllocatedActuatorMixer(control_cb, cb_handle, index);
} else {
return nullptr;
}
}
unsigned
AllocatedActuatorMixer::mix(float *outputs, unsigned space)
{
if (space < 1) {
return 0;
}
_control_cb(_cb_handle,
_control_group,
_control_index,
*outputs);
return 1;
}
void
AllocatedActuatorMixer::groups_required(uint32_t &groups)
{
groups |= 1 << _control_group;
}

103
src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp Normal file
View File

@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (C) 2012-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 mixer_AllocatedActuatorMixer.hpp
*
* Mixer for allocated actuators.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <lib/mixer/MixerBase/Mixer.hpp>
/**
* Mixer for allocated actuators.
*
* Copies a single actuator to a single output.
*/
class AllocatedActuatorMixer : public Mixer
{
public:
/**
* Constructor
*
* @param index Actuator index (0..15)
*/
AllocatedActuatorMixer(ControlCallback control_cb,
uintptr_t cb_handle,
uint8_t index);
virtual ~AllocatedActuatorMixer() = default;
// no copy, assignment, move, move assignment
AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete;
AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete;
AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete;
AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete;
/**
* Factory method with full external configuration.
*
* Given a pointer to a buffer containing a text description of the mixer,
* returns a pointer to a new instance of the mixer.
*
* @param control_cb The callback to invoke when fetching a
* control value.
* @param cb_handle Handle passed to the control callback.
* @param buf Buffer containing a text description of
* the mixer.
* @param buflen Length of the buffer in bytes, adjusted
* to reflect the bytes consumed.
* @return A new AllocatedActuatorMixer instance, or nullptr
* if the text format is bad.
*/
static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
const char *buf,
unsigned &buflen);
unsigned mix(float *outputs, unsigned space) override;
void groups_required(uint32_t &groups) override;
unsigned set_trim(float trim) override;
unsigned get_trim(float *trim) override;
private:
uint8_t _control_group; /**< group from which the input reads */
uint8_t _control_index; /**< index within the control group */
static int parse(const char *buf,
unsigned &buflen,
uint8_t &index);
};

39
src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
############################################################################
#
# 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
# 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_library(AllocatedActuatorMixer
AllocatedActuatorMixer.cpp
AllocatedActuatorMixer.hpp
)
target_link_libraries(AllocatedActuatorMixer PRIVATE MixerBase)
add_dependencies(AllocatedActuatorMixer prebuild_targets)

2
src/lib/mixer/CMakeLists.txt
View File

@ -34,6 +34,7 @@
# required by other mixers
add_subdirectory(MixerBase)
add_subdirectory(AllocatedActuatorMixer)
add_subdirectory(HelicopterMixer)
add_subdirectory(MultirotorMixer)
add_subdirectory(NullMixer)
@ -48,6 +49,7 @@ add_library(mixer
target_link_libraries(mixer
PRIVATE
AllocatedActuatorMixer
HelicopterMixer
MultirotorMixer
NullMixer

5
src/lib/mixer/MixerGroup.cpp
View File

@ -39,6 +39,7 @@
#include "MixerGroup.hpp"
#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp"
#include "HelicopterMixer/HelicopterMixer.hpp"
#include "MultirotorMixer/MultirotorMixer.hpp"
#include "NullMixer/NullMixer.hpp"
@ -192,6 +193,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
m = NullMixer::from_text(p, resid);
break;
case 'A':
m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid);
break;
case 'M':
m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
break;

20
src/lib/mixer_module/mixer_module.cpp
View File

@ -49,7 +49,9 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter
{&interface, ORB_ID(actuator_controls_0)},
{&interface, ORB_ID(actuator_controls_1)},
{&interface, ORB_ID(actuator_controls_2)},
{&interface, ORB_ID(actuator_controls_3)}
{&interface, ORB_ID(actuator_controls_3)},
{&interface, ORB_ID(actuator_controls_4)},
{&interface, ORB_ID(actuator_controls_5)},
},
_scheduling_policy(scheduling_policy),
_support_esc_calibration(support_esc_calibration),
@ -536,9 +538,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
/* motor spinup phase - lock throttle to zero */
if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) ||
(control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
@ -548,9 +552,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
/* throttle not arming - mark throttle input as invalid */
if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) ||
(control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 ||
control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) {
/* set the throttle to an invalid value */
input = NAN;
}

14
src/lib/parameters/px4params/srcparser.py
View File

@ -333,7 +333,7 @@ class SourceParser(object):
self.param_groups[group].AddParameter(param)
state = None
return True
def IsNumber(self, numberString):
try:
float(numberString)
@ -348,8 +348,8 @@ class SourceParser(object):
seenParamNames = []
#allowedUnits should match set defined in /Firmware/validation/module_schema.yaml
allowedUnits = set ([
'%', 'Hz', 'mAh',
'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m','rad s/m',
'%', 'Hz', '1/s', 'mAh',
'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m',
'bit/s', 'B/s',
'deg', 'deg*1e7', 'deg/s',
'celcius', 'gauss', 'gauss/s', 'gauss^2',
@ -357,9 +357,9 @@ class SourceParser(object):
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
'Ohm', 'V',
'us', 'ms', 's',
'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad',
'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
'N/(m/s)', 'Nm/(rad/s)', 'Nm', 'N',
'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad',
'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
'N/(m/s)', 'Nm/rad', 'Nm/(rad/s)', 'Nm', 'N',
'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD',''])
for group in self.GetParamGroups():
for param in group.GetParams():
@ -380,7 +380,7 @@ class SourceParser(object):
min = param.GetFieldValue("min")
max = param.GetFieldValue("max")
units = param.GetFieldValue("unit")
if units not in allowedUnits:
if units not in allowedUnits:
sys.stderr.write("Invalid unit in {0}: {1}\n".format(name, units))
return False
#sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max))

115
src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp Normal file
View File

@ -0,0 +1,115 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file AngularVelocityControl.cpp
*/
#include <AngularVelocityControl.hpp>
#include <px4_platform_common/defines.h>
using namespace matrix;
void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
_gain_p = P;
_gain_i = I;
_gain_d = D;
}
void AngularVelocityControl::setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative)
{
_saturation_positive = saturation_positive;
_saturation_negative = saturation_negative;
}
void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp,
const Vector3f &angular_acceleration, const float dt, const bool landed)
{
// angular rates error
Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity;
// P + D control
_angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration);
// I + FF control
Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp);
// compute torque setpoint
_torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity);
// update integral only if we are not landed
if (!landed) {
updateIntegral(angular_velocity_error, dt);
}
}
void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt)
{
for (int i = 0; i < 3; i++) {
// prevent further positive control saturation
if (_saturation_positive(i)) {
angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f);
}
// prevent further negative control saturation
if (_saturation_negative(i)) {
angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f);
}
// I term factor: reduce the I gain with increasing rate error.
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
// change (noticeable in a bounce-back effect after a flip).
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
// and up to 200 deg error leads to <25% reduction of I.
float i_factor = angular_velocity_error(i) / math::radians(400.f);
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
// Perform the integration using a first order method
float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt;
// do not propagate the result if out of range or invalid
if (PX4_ISFINITE(angular_velocity_i)) {
_angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i));
}
}
}
void AngularVelocityControl::reset()
{
_angular_velocity_int.zero();
_torque_sp.zero();
_angular_accel_sp.zero();
}

149
src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp Normal file
View File

@ -0,0 +1,149 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file AngularVelocityControl.hpp
*
* PID 3 axis angular velocity control.
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
class AngularVelocityControl
{
public:
AngularVelocityControl() = default;
~AngularVelocityControl() = default;
/**
* Set the control gains
* @param P 3D vector of proportional gains for body x,y,z axis
* @param I 3D vector of integral gains
* @param D 3D vector of derivative gains
*/
void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
/**
* Set the mximum absolute value of the integrator for all axes
* @param integrator_limit limit value for all axes x, y, z
*/
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
/**
* Set direct angular velocity setpoint to torque feed forward gain
* @see _gain_ff
* @param FF 3D vector of feed forward gains for body x,y,z axis
*/
void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };
/**
* Set inertia matrix
* @see _inertia
* @param inertia inertia matrix
*/
void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; };
/**
* Set saturation status
* @see _saturation_positive
* @see _saturation_negative
* @param saturation_positive positive saturation
* @param saturation_negative negative saturation
*/
void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
const matrix::Vector<bool, 3> &saturation_negative);
/**
* Run one control loop cycle calculation
* @param angular_velocity estimation of the current vehicle angular velocity
* @param angular_velocity_sp desired vehicle angular velocity setpoint
* @param angular_acceleration estimation of the current vehicle angular acceleration
* @param dt elapsed time since last update
* @param landed whether the vehicle is on the ground
*/
void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp,
const matrix::Vector3f &angular_acceleration, const float dt, const bool landed);
/**
* Get the desired angular acceleration
* @see _angular_accel_sp
*/
const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;};
/**
* Get the torque vector to apply to the vehicle
* @see _torque_sp
*/
const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;};
/**
* Get the integral term
* @see _torque_sp
*/
const matrix::Vector3f &getIntegral() {return _angular_velocity_int;};
/**
* Set the integral term to 0 to prevent windup
* @see _angular_velocity_int
*/
void resetIntegral() { _angular_velocity_int.zero(); }
/**
* ReSet the controller state
*/
void reset();
private:
void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt);
// Gains
matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z
matrix::Vector3f _gain_i; ///< integral gain
matrix::Vector3f _gain_d; ///< derivative gain
matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain
matrix::Matrix3f _inertia{matrix::eye<float, 3>()}; ///< inertia matrix
// States
matrix::Vector3f _angular_velocity_int;
matrix::Vector<bool, 3> _saturation_positive;
matrix::Vector<bool, 3> _saturation_negative;
// Output
matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains
matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle
};

45
src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp Normal file
View File

@ -0,0 +1,45 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <AngularVelocityControl.hpp>
using namespace matrix;
TEST(AngularVelocityControlTest, AllZeroCase)
{
AngularVelocityControl control;
control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
Vector3f torque = control.getTorqueSetpoint();
EXPECT_EQ(torque, Vector3f());
}

41
src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(AngularVelocityControl
AngularVelocityControl.cpp
)
target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(AngularVelocityControl PRIVATE mathlib)
px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl)

344
src/modules/angular_velocity_controller/AngularVelocityController.cpp Normal file
View File

@ -0,0 +1,344 @@
/****************************************************************************
*
* 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 "AngularVelocityController.hpp"
#include <drivers/drv_hrt.h>
#include <circuit_breaker/circuit_breaker.h>
#include <mathlib/math/Limits.hpp>
#include <mathlib/math/Functions.hpp>
#include <ecl/geo/geo.h>
using namespace matrix;
using namespace time_literals;
AngularVelocityController::AngularVelocityController() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
parameters_updated();
}
AngularVelocityController::~AngularVelocityController()
{
perf_free(_loop_perf);
}
bool
AngularVelocityController::init()
{
if (!_vehicle_angular_velocity_sub.registerCallback()) {
PX4_ERR("vehicle_angular_velocity callback registration failed!");
return false;
}
return true;
}
void
AngularVelocityController::parameters_updated()
{
// 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 k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get());
_control.setGains(
k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())),
k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())),
k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get())));
_control.setIntegratorLimit(
Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get()));
_control.setFeedForwardGain(
Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get()));
// inertia matrix
const float inertia[3][3] = {
{_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()},
{_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()},
{_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()}
};
_control.setInertiaMatrix(matrix::Matrix3f(inertia));
// Hover thrust
if (!_param_mpc_use_hte.get()) {
_hover_thrust = _param_mpc_thr_hover.get();
}
}
void
AngularVelocityController::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(&param_update);
updateParams();
parameters_updated();
}
/* run controller on gyro changes */
vehicle_angular_velocity_s vehicle_angular_velocity;
if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
const hrt_abstime now = hrt_absolute_time();
_timestamp_sample = vehicle_angular_velocity.timestamp_sample;
// 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 angular_velocity{vehicle_angular_velocity.xyz};
/* check for updates in other topics */
_vehicle_status_sub.update(&_vehicle_status);
_vehicle_control_mode_sub.update(&_vehicle_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;
}
}
// Check for updates of hover thrust
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
if (_hover_thrust_estimate_sub.update(&hte)) {
_hover_thrust = hte.hover_thrust;
}
}
// check angular acceleration topic
vehicle_angular_acceleration_s vehicle_angular_acceleration;
if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) {
_angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz);
}
// check rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint;
if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
_angular_velocity_sp(0) = vehicle_rates_setpoint.roll;
_angular_velocity_sp(1) = vehicle_rates_setpoint.pitch;
_angular_velocity_sp(2) = vehicle_rates_setpoint.yaw;
_thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body);
// Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G
_thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust);
}
// run the controller
if (_vehicle_control_mode.flag_control_rates_enabled) {
// reset integral if disarmed
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_control.resetIntegral();
}
// update saturation status from mixer feedback
if (_control_allocator_status_sub.updated()) {
control_allocator_status_s control_allocator_status;
if (_control_allocator_status_sub.copy(&control_allocator_status)) {
Vector<bool, 3> saturation_positive;
Vector<bool, 3> saturation_negative;
if (not control_allocator_status.torque_setpoint_achieved) {
for (size_t i = 0; i < 3; i++) {
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
saturation_positive(i) = true;
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
saturation_negative(i) = true;
}
}
}
_control.setSaturationStatus(saturation_positive, saturation_negative);
}
}
// run rate controller
_control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
Vector3f integral = _control.getIntegral();
rate_ctrl_status.timestamp = hrt_absolute_time();
rate_ctrl_status.rollspeed_integ = integral(0);
rate_ctrl_status.pitchspeed_integ = integral(1);
rate_ctrl_status.yawspeed_integ = integral(2);
_rate_ctrl_status_pub.publish(rate_ctrl_status);
// publish controller output
publish_angular_acceleration_setpoint();
publish_torque_setpoint();
publish_thrust_setpoint();
}
}
perf_end(_loop_perf);
}
void
AngularVelocityController::publish_angular_acceleration_setpoint()
{
Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
v_angular_accel_sp.timestamp = hrt_absolute_time();
v_angular_accel_sp.timestamp_sample = _timestamp_sample;
v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
}
void
AngularVelocityController::publish_torque_setpoint()
{
Vector3f torque_sp = _control.getTorqueSetpoint();
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = _timestamp_sample;
v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void
AngularVelocityController::publish_thrust_setpoint()
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = _timestamp_sample;
v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
int AngularVelocityController::task_spawn(int argc, char *argv[])
{
AngularVelocityController *instance = new AngularVelocityController();
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 AngularVelocityController::print_status()
{
PX4_INFO("Running");
perf_print_counter(_loop_perf);
return 0;
}
int AngularVelocityController::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int AngularVelocityController::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements the angular velocity controller.
It takes angular velocity setpoints and measured angular
velocity as inputs and outputs actuator setpoints.
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;
}
/**
* Angular velocity controller app start / stop handling function
*/
extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]);
int angular_velocity_controller_main(int argc, char *argv[])
{
return AngularVelocityController::main(argc, argv);
}

177
src/modules/angular_velocity_controller/AngularVelocityController.hpp Normal file
View File

@ -0,0 +1,177 @@
/****************************************************************************
*
* 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 <AngularVelocityControl.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#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/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_acceleration_setpoint.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>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
class AngularVelocityController : public ModuleBase<AngularVelocityController>, public ModuleParams,
public px4::WorkItem
{
public:
AngularVelocityController();
virtual ~AngularVelocityController();
/** @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();
void publish_angular_acceleration_setpoint();
void publish_torque_setpoint();
void publish_thrust_setpoint();
void publish_actuator_controls();
AngularVelocityControl _control; ///< class for control calculations
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; /**< vehicle angular acceleration subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */
uORB::Publication<vehicle_angular_acceleration_setpoint_s> _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
bool _landed{true};
bool _maybe_landed{true};
float _battery_status_scale{0.0f};
perf_counter_t _loop_perf; /**< loop duration performance counter */
matrix::Vector3f _angular_velocity_sp; /**< angular velocity setpoint */
matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */
matrix::Vector3f _thrust_sp; /**< thrust setpoint */
float _hover_thrust{0.5f}; /**< Normalized hover thrust **/
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};
hrt_abstime _timestamp_sample{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::AVC_X_P>) _param_avc_x_p,
(ParamFloat<px4::params::AVC_X_I>) _param_avc_x_i,
(ParamFloat<px4::params::AVC_X_I_LIM>) _param_avc_x_i_lim,
(ParamFloat<px4::params::AVC_X_D>) _param_avc_x_d,
(ParamFloat<px4::params::AVC_X_FF>) _param_avc_x_ff,
(ParamFloat<px4::params::AVC_X_K>) _param_avc_x_k,
(ParamFloat<px4::params::AVC_Y_P>) _param_avc_y_p,
(ParamFloat<px4::params::AVC_Y_I>) _param_avc_y_i,
(ParamFloat<px4::params::AVC_Y_I_LIM>) _param_avc_y_i_lim,
(ParamFloat<px4::params::AVC_Y_D>) _param_avc_y_d,
(ParamFloat<px4::params::AVC_Y_FF>) _param_avc_y_ff,
(ParamFloat<px4::params::AVC_Y_K>) _param_avc_y_k,
(ParamFloat<px4::params::AVC_Z_P>) _param_avc_z_p,
(ParamFloat<px4::params::AVC_Z_I>) _param_avc_z_i,
(ParamFloat<px4::params::AVC_Z_I_LIM>) _param_avc_z_i_lim,
(ParamFloat<px4::params::AVC_Z_D>) _param_avc_z_d,
(ParamFloat<px4::params::AVC_Z_FF>) _param_avc_z_ff,
(ParamFloat<px4::params::AVC_Z_K>) _param_avc_z_k,
(ParamFloat<px4::params::VM_MASS>) _param_vm_mass,
(ParamFloat<px4::params::VM_INERTIA_XX>) _param_vm_inertia_xx,
(ParamFloat<px4::params::VM_INERTIA_YY>) _param_vm_inertia_yy,
(ParamFloat<px4::params::VM_INERTIA_ZZ>) _param_vm_inertia_zz,
(ParamFloat<px4::params::VM_INERTIA_XY>) _param_vm_inertia_xy,
(ParamFloat<px4::params::VM_INERTIA_XZ>) _param_vm_inertia_xz,
(ParamFloat<px4::params::VM_INERTIA_YZ>) _param_vm_inertia_yz,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte
)
};

49
src/modules/angular_velocity_controller/CMakeLists.txt Normal file
View File

@ -0,0 +1,49 @@
############################################################################
#
# 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(AngularVelocityControl)
px4_add_module(
MODULE modules__angular_velocity_control
MAIN angular_velocity_controller
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
AngularVelocityController.cpp
AngularVelocityController.hpp
DEPENDS
circuit_breaker
mathlib
AngularVelocityControl
px4_work_queue
)

297
src/modules/angular_velocity_controller/angular_velocity_controller_params.c Normal file
View File

@ -0,0 +1,297 @@
/****************************************************************************
*
* 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 angular_velocity_controller_params.c
* Parameters for angular velocity controller.
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Body X axis angular velocity P gain
*
* Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_P, 18.f);
/**
* Body X axis angular velocity I gain
*
* Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f);
/**
* Body X axis angular velocity integrator limit
*
* Body X axis angular velocity 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.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f);
/**
* Body X axis angular velocity D gain
*
* Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f);
/**
* Body X axis angular velocity feedforward gain
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f);
/**
* Body X axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_X_K * (AVC_X_P * error
* + AVC_X_I * error_integral
* + AVC_X_D * error_derivative)
* Set AVC_X_P=1 to implement a PID in the ideal form.
* Set AVC_X_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 5.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f);
/**
* Body Y axis angular velocity P gain
*
* Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f);
/**
* Body Y axis angular velocity I gain
*
* Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f);
/**
* Body Y axis angular velocity integrator limit
*
* Body Y axis angular velocity 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.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f);
/**
* Body Y axis angular velocity D gain
*
* Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f);
/**
* Body Y axis angular velocity feedforward
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f);
/**
* Body Y axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_Y_K * (AVC_Y_P * error
* + AVC_Y_I * error_integral
* + AVC_Y_D * error_derivative)
* Set AVC_Y_P=1 to implement a PID in the ideal form.
* Set AVC_Y_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 20.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f);
/**
* Body Z axis angular velocity P gain
*
* Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s.
*
* @unit 1/s
* @min 0.0
* @max 20.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f);
/**
* Body Z axis angular velocity I gain
*
* Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset.
*
* @unit Nm/rad
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f);
/**
* Body Z axis angular velocity integrator limit
*
* Body Z axis angular velocity 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.
*
* @unit Nm
* @min 0.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f);
/**
* Body Z axis angular velocity D gain
*
* Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f);
/**
* Body Z axis angular velocity feedforward
*
* Improves tracking performance.
*
* @unit Nm/(rad/s)
* @min 0.0
* @decimal 4
* @increment 0.01
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f);
/**
* Body Z axis angular velocity controller gain
*
* Global gain of the controller.
*
* This gain scales the P, I and D terms of the controller:
* output = AVC_Z_K * (AVC_Z_P * error
* + AVC_Z_I * error_integral
* + AVC_Z_D * error_derivative)
* Set AVC_Z_P=1 to implement a PID in the ideal form.
* Set AVC_Z_K=1 to implement a PID in the parallel form.
*
* @min 0.0
* @max 5.0
* @decimal 4
* @increment 0.0005
* @group Angular Velocity Control
*/
PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f);

109
src/modules/angular_velocity_controller/vehicle_model_params.c Normal file
View File

@ -0,0 +1,109 @@
/****************************************************************************
*
* 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 vehicle_model_params.c
* Parameters for vehicle model.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Mass
*
* @unit kg
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_MASS, 1.f);
/**
* Inertia matrix, XX component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f);
/**
* Inertia matrix, YY component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f);
/**
* Inertia matrix, ZZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f);
/**
* Inertia matrix, XY component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f);
/**
* Inertia matrix, XZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f);
/**
* Inertia matrix, YZ component
*
* @unit kg m^2
* @decimal 5
* @increment 0.00001
* @group Vehicle Model
*/
PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f);

116
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectiveness.hpp
*
* Interface for Actuator Effectiveness
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <ControlAllocation/ControlAllocation.hpp>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_actuator_setpoint.h>
class ActuatorEffectiveness
{
public:
ActuatorEffectiveness() = default;
virtual ~ActuatorEffectiveness() = default;
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
enum class FlightPhase {
HOVER_FLIGHT = 0,
FORWARD_FLIGHT = 1,
TRANSITION_HF_TO_FF = 2,
TRANSITION_FF_TO_HF = 3
};
/**
* Update effectiveness matrix
*
* @return True if the effectiveness matrix has changed
*/
virtual bool update() = 0;
/**
* Set the current flight phase
*
* @param Flight phase
*/
virtual void setFlightPhase(const FlightPhase &flight_phase)
{
_flight_phase = flight_phase;
};
/**
* Get the control effectiveness matrix
*
* @return Effectiveness matrix
*/
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const
{
return _effectiveness;
};
/**
* Get the actuator trims
*
* @return Actuator trims
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorTrim() const
{
return _trim;
};
/**
* Get the current flight phase
*
* @return Flight phase
*/
const FlightPhase &getFlightPhase() const
{
return _flight_phase;
};
protected:
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; //< Effectiveness matrix
matrix::Vector<float, NUM_ACTUATORS> _trim; //< Actuator trim
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; //< Current flight phase
};

201
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp Normal file
View File

@ -0,0 +1,201 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessMultirotor.hpp
*
* Actuator effectiveness computed from rotors position and orientation
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ActuatorEffectivenessMultirotor.hpp"
ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor():
ModuleParams(nullptr)
{
parameters_updated();
}
bool
ActuatorEffectivenessMultirotor::update()
{
bool updated = false;
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
updated = true;
}
return updated;
}
matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry)
{
matrix::Matrix<float, ActuatorEffectivenessMultirotor::NUM_AXES, ActuatorEffectivenessMultirotor::NUM_ACTUATORS>
effectiveness;
for (size_t i = 0; i < NUM_ROTORS_MAX; i++) {
// Get rotor axis
matrix::Vector3f axis(
geometry.rotors[i].axis_x,
geometry.rotors[i].axis_y,
geometry.rotors[i].axis_z
);
// Normalize axis
float axis_norm = axis.norm();
if (axis_norm > FLT_EPSILON) {
axis /= axis_norm;
} else {
// Bad axis definition, ignore this rotor
continue;
}
// Get rotor position
matrix::Vector3f position(
geometry.rotors[i].position_x,
geometry.rotors[i].position_y,
geometry.rotors[i].position_z
);
// Get coefficients
float ct = geometry.rotors[i].thrust_coef;
float km = geometry.rotors[i].moment_ratio;
// Compute thrust generated by this rotor
matrix::Vector3f thrust = ct * axis;
// Compute moment generated by this rotor
matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
// Fill corresponding items in effectiveness matrix
for (size_t j = 0; j < 3; j++) {
effectiveness(j, i) = moment(j);
effectiveness(j + 3, i) = thrust(j);
}
}
return effectiveness;
}
void
ActuatorEffectivenessMultirotor::parameters_updated()
{
// Get multirotor geometry
MultirotorGeometry geometry = {};
geometry.rotors[0].position_x = _param_ca_mc_r0_px.get();
geometry.rotors[0].position_y = _param_ca_mc_r0_py.get();
geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get();
geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get();
geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get();
geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get();
geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get();
geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get();
geometry.rotors[1].position_x = _param_ca_mc_r1_px.get();
geometry.rotors[1].position_y = _param_ca_mc_r1_py.get();
geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get();
geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get();
geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get();
geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get();
geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get();
geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get();
geometry.rotors[2].position_x = _param_ca_mc_r2_px.get();
geometry.rotors[2].position_y = _param_ca_mc_r2_py.get();
geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get();
geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get();
geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get();
geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get();
geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get();
geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get();
geometry.rotors[3].position_x = _param_ca_mc_r3_px.get();
geometry.rotors[3].position_y = _param_ca_mc_r3_py.get();
geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get();
geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get();
geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get();
geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get();
geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get();
geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get();
geometry.rotors[4].position_x = _param_ca_mc_r4_px.get();
geometry.rotors[4].position_y = _param_ca_mc_r4_py.get();
geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get();
geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get();
geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get();
geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get();
geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get();
geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get();
geometry.rotors[5].position_x = _param_ca_mc_r5_px.get();
geometry.rotors[5].position_y = _param_ca_mc_r5_py.get();
geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get();
geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get();
geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get();
geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get();
geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get();
geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get();
geometry.rotors[6].position_x = _param_ca_mc_r6_px.get();
geometry.rotors[6].position_y = _param_ca_mc_r6_py.get();
geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get();
geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get();
geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get();
geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get();
geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get();
geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get();
geometry.rotors[7].position_x = _param_ca_mc_r7_px.get();
geometry.rotors[7].position_y = _param_ca_mc_r7_py.get();
geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get();
geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get();
geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get();
geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get();
geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get();
geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get();
// Compute effectiveness matrix
_effectiveness = computeEffectivenessMatrix(geometry);
}

163
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp Normal file
View File

@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessMultirotor.hpp
*
* Actuator effectiveness computed from rotors position and orientation
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "ActuatorEffectiveness.hpp"
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness
{
public:
ActuatorEffectivenessMultirotor();
virtual ~ActuatorEffectivenessMultirotor() = default;
/**
* Update effectiveness matrix
*
* @return True if the effectiveness matrix has changed
*/
virtual bool update() override;
static constexpr int NUM_ROTORS_MAX = 8;
typedef struct {
float position_x;
float position_y;
float position_z;
float axis_x;
float axis_y;
float axis_z;
float thrust_coef;
float moment_ratio;
} RotorGeometry;
typedef struct {
RotorGeometry rotors[NUM_ROTORS_MAX];
} MultirotorGeometry;
static matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> computeEffectivenessMatrix(MultirotorGeometry);
private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::CA_MC_R0_PX>) _param_ca_mc_r0_px,
(ParamFloat<px4::params::CA_MC_R0_PY>) _param_ca_mc_r0_py,
(ParamFloat<px4::params::CA_MC_R0_PZ>) _param_ca_mc_r0_pz,
(ParamFloat<px4::params::CA_MC_R0_AX>) _param_ca_mc_r0_ax,
(ParamFloat<px4::params::CA_MC_R0_AY>) _param_ca_mc_r0_ay,
(ParamFloat<px4::params::CA_MC_R0_AZ>) _param_ca_mc_r0_az,
(ParamFloat<px4::params::CA_MC_R0_CT>) _param_ca_mc_r0_ct,
(ParamFloat<px4::params::CA_MC_R0_KM>) _param_ca_mc_r0_km,
(ParamFloat<px4::params::CA_MC_R1_PX>) _param_ca_mc_r1_px,
(ParamFloat<px4::params::CA_MC_R1_PY>) _param_ca_mc_r1_py,
(ParamFloat<px4::params::CA_MC_R1_PZ>) _param_ca_mc_r1_pz,
(ParamFloat<px4::params::CA_MC_R1_AX>) _param_ca_mc_r1_ax,
(ParamFloat<px4::params::CA_MC_R1_AY>) _param_ca_mc_r1_ay,
(ParamFloat<px4::params::CA_MC_R1_AZ>) _param_ca_mc_r1_az,
(ParamFloat<px4::params::CA_MC_R1_CT>) _param_ca_mc_r1_ct,
(ParamFloat<px4::params::CA_MC_R1_KM>) _param_ca_mc_r1_km,
(ParamFloat<px4::params::CA_MC_R2_PX>) _param_ca_mc_r2_px,
(ParamFloat<px4::params::CA_MC_R2_PY>) _param_ca_mc_r2_py,
(ParamFloat<px4::params::CA_MC_R2_PZ>) _param_ca_mc_r2_pz,
(ParamFloat<px4::params::CA_MC_R2_AX>) _param_ca_mc_r2_ax,
(ParamFloat<px4::params::CA_MC_R2_AY>) _param_ca_mc_r2_ay,
(ParamFloat<px4::params::CA_MC_R2_AZ>) _param_ca_mc_r2_az,
(ParamFloat<px4::params::CA_MC_R2_CT>) _param_ca_mc_r2_ct,
(ParamFloat<px4::params::CA_MC_R2_KM>) _param_ca_mc_r2_km,
(ParamFloat<px4::params::CA_MC_R3_PX>) _param_ca_mc_r3_px,
(ParamFloat<px4::params::CA_MC_R3_PY>) _param_ca_mc_r3_py,
(ParamFloat<px4::params::CA_MC_R3_PZ>) _param_ca_mc_r3_pz,
(ParamFloat<px4::params::CA_MC_R3_AX>) _param_ca_mc_r3_ax,
(ParamFloat<px4::params::CA_MC_R3_AY>) _param_ca_mc_r3_ay,
(ParamFloat<px4::params::CA_MC_R3_AZ>) _param_ca_mc_r3_az,
(ParamFloat<px4::params::CA_MC_R3_CT>) _param_ca_mc_r3_ct,
(ParamFloat<px4::params::CA_MC_R3_KM>) _param_ca_mc_r3_km,
(ParamFloat<px4::params::CA_MC_R4_PX>) _param_ca_mc_r4_px,
(ParamFloat<px4::params::CA_MC_R4_PY>) _param_ca_mc_r4_py,
(ParamFloat<px4::params::CA_MC_R4_PZ>) _param_ca_mc_r4_pz,
(ParamFloat<px4::params::CA_MC_R4_AX>) _param_ca_mc_r4_ax,
(ParamFloat<px4::params::CA_MC_R4_AY>) _param_ca_mc_r4_ay,
(ParamFloat<px4::params::CA_MC_R4_AZ>) _param_ca_mc_r4_az,
(ParamFloat<px4::params::CA_MC_R4_CT>) _param_ca_mc_r4_ct,
(ParamFloat<px4::params::CA_MC_R4_KM>) _param_ca_mc_r4_km,
(ParamFloat<px4::params::CA_MC_R5_PX>) _param_ca_mc_r5_px,
(ParamFloat<px4::params::CA_MC_R5_PY>) _param_ca_mc_r5_py,
(ParamFloat<px4::params::CA_MC_R5_PZ>) _param_ca_mc_r5_pz,
(ParamFloat<px4::params::CA_MC_R5_AX>) _param_ca_mc_r5_ax,
(ParamFloat<px4::params::CA_MC_R5_AY>) _param_ca_mc_r5_ay,
(ParamFloat<px4::params::CA_MC_R5_AZ>) _param_ca_mc_r5_az,
(ParamFloat<px4::params::CA_MC_R5_CT>) _param_ca_mc_r5_ct,
(ParamFloat<px4::params::CA_MC_R5_KM>) _param_ca_mc_r5_km,
(ParamFloat<px4::params::CA_MC_R6_PX>) _param_ca_mc_r6_px,
(ParamFloat<px4::params::CA_MC_R6_PY>) _param_ca_mc_r6_py,
(ParamFloat<px4::params::CA_MC_R6_PZ>) _param_ca_mc_r6_pz,
(ParamFloat<px4::params::CA_MC_R6_AX>) _param_ca_mc_r6_ax,
(ParamFloat<px4::params::CA_MC_R6_AY>) _param_ca_mc_r6_ay,
(ParamFloat<px4::params::CA_MC_R6_AZ>) _param_ca_mc_r6_az,
(ParamFloat<px4::params::CA_MC_R6_CT>) _param_ca_mc_r6_ct,
(ParamFloat<px4::params::CA_MC_R6_KM>) _param_ca_mc_r6_km,
(ParamFloat<px4::params::CA_MC_R7_PX>) _param_ca_mc_r7_px,
(ParamFloat<px4::params::CA_MC_R7_PY>) _param_ca_mc_r7_py,
(ParamFloat<px4::params::CA_MC_R7_PZ>) _param_ca_mc_r7_pz,
(ParamFloat<px4::params::CA_MC_R7_AX>) _param_ca_mc_r7_ax,
(ParamFloat<px4::params::CA_MC_R7_AY>) _param_ca_mc_r7_ay,
(ParamFloat<px4::params::CA_MC_R7_AZ>) _param_ca_mc_r7_az,
(ParamFloat<px4::params::CA_MC_R7_CT>) _param_ca_mc_r7_ct,
(ParamFloat<px4::params::CA_MC_R7_KM>) _param_ca_mc_r7_km
)
};

560
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c Normal file
View File

@ -0,0 +1,560 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessMultirotorParams.c
*
* Parameters for the actuator effectiveness of multirotors.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Position of rotor 0 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PX, 0.0);
/**
* Position of rotor 0 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PY, 0.0);
/**
* Position of rotor 0 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_PZ, 0.0);
/**
* Axis of rotor 0 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AX, 0.0);
/**
* Axis of rotor 0 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AY, 0.0);
/**
* Axis of rotor 0 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0);
/**
* Thrust coefficient of rotor 0
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT0_MIN and CA_ACT0_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_CT, 0.0);
/**
* Moment coefficient of rotor 0
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.05);
/**
* Position of rotor 1 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PX, 0.0);
/**
* Position of rotor 1 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PY, 0.0);
/**
* Position of rotor 1 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_PZ, 0.0);
/**
* Axis of rotor 1 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AX, 0.0);
/**
* Axis of rotor 1 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AY, 0.0);
/**
* Axis of rotor 1 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0);
/**
* Thrust coefficient of rotor 1
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT1_MIN and CA_ACT1_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_CT, 0.0);
/**
* Moment coefficient of rotor 1
*
* The moment coefficient if defined as Torque = KM * Thrust,
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.05);
/**
* Position of rotor 2 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PX, 0.0);
/**
* Position of rotor 2 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PY, 0.0);
/**
* Position of rotor 2 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_PZ, 0.0);
/**
* Axis of rotor 2 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AX, 0.0);
/**
* Axis of rotor 2 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AY, 0.0);
/**
* Axis of rotor 2 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0);
/**
* Thrust coefficient of rotor 2
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT2_MIN and CA_ACT2_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_CT, 0.0);
/**
* Moment coefficient of rotor 2
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.05);
/**
* Position of rotor 3 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PX, 0.0);
/**
* Position of rotor 3 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PY, 0.0);
/**
* Position of rotor 3 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_PZ, 0.0);
/**
* Axis of rotor 3 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AX, 0.0);
/**
* Axis of rotor 3 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AY, 0.0);
/**
* Axis of rotor 3 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0);
/**
* Thrust coefficient of rotor 3
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT3_MIN and CA_ACT3_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_CT, 0.0);
/**
* Moment coefficient of rotor 3
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.05);
/**
* Position of rotor 4 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PX, 0.0);
/**
* Position of rotor 4 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PY, 0.0);
/**
* Position of rotor 4 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_PZ, 0.0);
/**
* Axis of rotor 4 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AX, 0.0);
/**
* Axis of rotor 4 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AY, 0.0);
/**
* Axis of rotor 4 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0);
/**
* Thrust coefficient of rotor 4
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT4_MIN and CA_ACT4_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_CT, 0.0);
/**
* Moment coefficient of rotor 4
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.05);
/**
* Position of rotor 5 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PX, 0.0);
/**
* Position of rotor 5 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PY, 0.0);
/**
* Position of rotor 5 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_PZ, 0.0);
/**
* Axis of rotor 5 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AX, 0.0);
/**
* Axis of rotor 5 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AY, 0.0);
/**
* Axis of rotor 5 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0);
/**
* Thrust coefficient of rotor 5
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT5_MIN and CA_ACT5_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_CT, 0.0);
/**
* Moment coefficient of rotor 5
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.05);
/**
* Position of rotor 6 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PX, 0.0);
/**
* Position of rotor 6 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PY, 0.0);
/**
* Position of rotor 6 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_PZ, 0.0);
/**
* Axis of rotor 6 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AX, 0.0);
/**
* Axis of rotor 6 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AY, 0.0);
/**
* Axis of rotor 6 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0);
/**
* Thrust coefficient of rotor 6
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT6_MIN and CA_ACT6_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_CT, 0.0);
/**
* Moment coefficient of rotor 6
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.05);
/**
* Position of rotor 7 along X body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PX, 0.0);
/**
* Position of rotor 7 along Y body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PY, 0.0);
/**
* Position of rotor 7 along Z body axis
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_PZ, 0.0);
/**
* Axis of rotor 7 thrust vector, X body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AX, 0.0);
/**
* Axis of rotor 7 thrust vector, Y body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AY, 0.0);
/**
* Axis of rotor 7 thrust vector, Z body axis component
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0);
/**
* Thrust coefficient of rotor 7
*
* The thrust coefficient if defined as Thrust = CT * u^2,
* where u (with value between CA_ACT7_MIN and CA_ACT7_MAX)
* is the output signal sent to the motor controller.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_CT, 0.0);
/**
* Moment coefficient of rotor 7
*
* The moment coefficient if defined as Torque = KM * Thrust
*
* Use a positive value for a rotor with CCW rotation.
* Use a negative value for a rotor with CW rotation.
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.05);

102
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp Normal file
View File

@ -0,0 +1,102 @@
/****************************************************************************
*
* Copyright (C) 2020 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 ActuatorEffectivenessMultirotorTest.cpp
*
* Tests for Control Allocation Algorithms
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include <gtest/gtest.h>
#include <ActuatorEffectivenessMultirotor.hpp>
using namespace matrix;
TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase)
{
// Quad wide geometry
ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {};
geometry.rotors[0].position_x = 1.0f;
geometry.rotors[0].position_y = 1.0f;
geometry.rotors[0].position_z = 0.0f;
geometry.rotors[0].axis_x = 0.0f;
geometry.rotors[0].axis_y = 0.0f;
geometry.rotors[0].axis_z = -1.0f;
geometry.rotors[0].thrust_coef = 1.0f;
geometry.rotors[0].moment_ratio = 0.05f;
geometry.rotors[1].position_x = -1.0f;
geometry.rotors[1].position_y = -1.0f;
geometry.rotors[1].position_z = 0.0f;
geometry.rotors[1].axis_x = 0.0f;
geometry.rotors[1].axis_y = 0.0f;
geometry.rotors[1].axis_z = -1.0f;
geometry.rotors[1].thrust_coef = 1.0f;
geometry.rotors[1].moment_ratio = 0.05f;
geometry.rotors[2].position_x = 1.0f;
geometry.rotors[2].position_y = -1.0f;
geometry.rotors[2].position_z = 0.0f;
geometry.rotors[2].axis_x = 0.0f;
geometry.rotors[2].axis_y = 0.0f;
geometry.rotors[2].axis_z = -1.0f;
geometry.rotors[2].thrust_coef = 1.0f;
geometry.rotors[2].moment_ratio = -0.05f;
geometry.rotors[3].position_x = -1.0f;
geometry.rotors[3].position_y = 1.0f;
geometry.rotors[3].position_z = 0.0f;
geometry.rotors[3].axis_x = 0.0f;
geometry.rotors[3].axis_y = 0.0f;
geometry.rotors[3].axis_z = -1.0f;
geometry.rotors[3].thrust_coef = 1.0f;
geometry.rotors[3].moment_ratio = -0.05f;
matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness =
ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry);
const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
{-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
matrix::Matrix<float, ActuatorEffectiveness::NUM_AXES, ActuatorEffectiveness::NUM_ACTUATORS> effectiveness_expected(
expected);
EXPECT_EQ(effectiveness, effectiveness_expected);
}

108
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp Normal file
View File

@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessStandardVTOL.hpp
*
* Actuator effectiveness for standard VTOL
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ActuatorEffectivenessStandardVTOL.hpp"
ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL()
{
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessStandardVTOL::update()
{
if (_updated) {
_updated = false;
return true;
}
return false;
}
void
ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
ActuatorEffectiveness::setFlightPhase(flight_phase);
_updated = true;
switch (_flight_phase) {
case FlightPhase::HOVER_FLIGHT: {
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
{-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
break;
}
case FlightPhase::FORWARD_FLIGHT: {
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
{ 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
break;
}
case FlightPhase::TRANSITION_HF_TO_FF:
case FlightPhase::TRANSITION_FF_TO_HF: {
const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = {
{ -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(standard_vtol);
break;
}
}
}

68
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessStandardVTOL.hpp
*
* Actuator effectiveness for standard VTOL
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "ActuatorEffectiveness.hpp"
class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness
{
public:
ActuatorEffectivenessStandardVTOL();
virtual ~ActuatorEffectivenessStandardVTOL() = default;
/**
* Update effectiveness matrix
*
* @return True if the effectiveness matrix has changed
*/
virtual bool update() override;
/**
* Set the current flight phase
*
* @param Flight phase
*/
void setFlightPhase(const FlightPhase &flight_phase) override;
protected:
bool _updated{false};
};

116
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessTiltrotorVTOL.hpp
*
* Actuator effectiveness for tiltrotor VTOL
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ActuatorEffectivenessTiltrotorVTOL.hpp"
ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL()
{
setFlightPhase(FlightPhase::HOVER_FLIGHT);
}
bool
ActuatorEffectivenessTiltrotorVTOL::update()
{
if (_updated) {
_updated = false;
return true;
}
return false;
}
void
ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
ActuatorEffectiveness::setFlightPhase(flight_phase);
_updated = true;
// Trim
float tilt = 0.0f;
switch (_flight_phase) {
case FlightPhase::HOVER_FLIGHT: {
tilt = 0.0f;
break;
}
case FlightPhase::FORWARD_FLIGHT: {
tilt = 1.5f;
break;
}
case FlightPhase::TRANSITION_FF_TO_HF:
case FlightPhase::TRANSITION_HF_TO_FF: {
tilt = 1.0f;
break;
}
}
// Trim: half throttle, tilted motors
_trim(0) = 0.5f;
_trim(1) = 0.5f;
_trim(2) = 0.5f;
_trim(3) = 0.5f;
_trim(4) = tilt;
_trim(5) = tilt;
_trim(6) = tilt;
_trim(7) = tilt;
// Effectiveness
const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = {
{-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
_effectiveness = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>(tiltrotor_vtol);
// Temporarily disable a few controls (WIP)
for (size_t j = 4; j < 8; j++) {
_effectiveness(3, j) = 0.0f;
_effectiveness(4, j) = 0.0f;
_effectiveness(5, j) = 0.0f;
}
}

68
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2020 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 ActuatorEffectivenessTiltrotorVTOL.hpp
*
* Actuator effectiveness for tiltrotor VTOL
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "ActuatorEffectiveness.hpp"
class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness
{
public:
ActuatorEffectivenessTiltrotorVTOL();
virtual ~ActuatorEffectivenessTiltrotorVTOL() = default;
/**
* Update effectiveness matrix
*
* @return True if the effectiveness matrix has changed
*/
virtual bool update() override;
/**
* Set the current flight phase
*
* @param Flight phase
*/
void setFlightPhase(const FlightPhase &flight_phase) override;
protected:
bool _updated{false};
};

52
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt Normal file
View File

@ -0,0 +1,52 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(ActuatorEffectiveness
ActuatorEffectiveness.hpp
ActuatorEffectivenessMultirotor.cpp
ActuatorEffectivenessMultirotor.hpp
ActuatorEffectivenessStandardVTOL.cpp
ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectivenessTiltrotorVTOL.hpp
)
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(ActuatorEffectiveness
PRIVATE
mathlib
ControlAllocation
)
# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness)

52
src/modules/control_allocator/CMakeLists.txt Normal file
View File

@ -0,0 +1,52 @@
############################################################################
#
# 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.
#
############################################################################
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(ActuatorEffectiveness)
add_subdirectory(ControlAllocation)
px4_add_module(
MODULE modules__control_allocator
MAIN control_allocator
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
ControlAllocator.cpp
ControlAllocator.hpp
DEPENDS
mathlib
ActuatorEffectiveness
ControlAllocation
mixer
px4_work_queue
)

46
src/modules/control_allocator/ControlAllocation/CMakeLists.txt Normal file
View File

@ -0,0 +1,46 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(ControlAllocation
ControlAllocation.cpp
ControlAllocation.hpp
ControlAllocationPseudoInverse.cpp
ControlAllocationPseudoInverse.hpp
ControlAllocationSequentialDesaturation.cpp
ControlAllocationSequentialDesaturation.hpp
)
target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(ControlAllocation PRIVATE mathlib)
px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation)

164
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp Normal file
View File

@ -0,0 +1,164 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocation.cpp
*
* Interface for Control Allocation Algorithms
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ControlAllocation.hpp"
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &
ControlAllocation::getActuatorSetpoint() const
{
return _actuator_sp;
}
void
ControlAllocation::setControlSetpoint(const matrix::Vector<float, ControlAllocation::NUM_AXES> &control)
{
_control_sp = control;
}
const matrix::Vector<float, ControlAllocation::NUM_AXES> &
ControlAllocation::getControlSetpoint() const
{
return _control_sp;
}
const matrix::Vector<float, ControlAllocation::NUM_AXES> &
ControlAllocation::getAllocatedControl() const
{
return _control_allocated;
}
void
ControlAllocation::setEffectivenessMatrix(
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim)
{
_effectiveness = effectiveness;
_actuator_trim = clipActuatorSetpoint(actuator_trim);
_control_trim = _effectiveness * _actuator_trim;
}
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &
ControlAllocation::getEffectivenessMatrix() const
{
return _effectiveness;
}
void
ControlAllocation::setActuatorMin(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
&actuator_min)
{
_actuator_min = actuator_min;
}
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &
ControlAllocation::getActuatorMin() const
{
return _actuator_min;
}
void
ControlAllocation::setActuatorMax(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
&actuator_max)
{
_actuator_max = actuator_max;
}
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &
ControlAllocation::getActuatorMax() const
{
return _actuator_max;
}
void
ControlAllocation::setActuatorSetpoint(
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_sp)
{
// Set actuator setpoint
_actuator_sp = actuator_sp;
// Clip
_actuator_sp = clipActuatorSetpoint(_actuator_sp);
// Compute achieved control
_control_allocated = _effectiveness * _actuator_sp;
}
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
ControlAllocation::clipActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator) const
{
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_clipped;
for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
if (_actuator_max(i) < _actuator_min(i)) {
actuator_clipped(i) = _actuator_trim(i);
} else if (actuator_clipped(i) < _actuator_min(i)) {
actuator_clipped(i) = _actuator_min(i);
} else if (actuator_clipped(i) > _actuator_max(i)) {
actuator_clipped(i) = _actuator_max(i);
} else {
actuator_clipped(i) = actuator(i);
}
}
return actuator_clipped;
}
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
const
{
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> actuator_normalized;
for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) {
if (_actuator_min(i) < _actuator_max(i)) {
actuator_normalized(i) = -1.0f + 2.0f * (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
} else {
actuator_normalized(i) = -1.0f + 2.0f * (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i));
}
}
return actuator_normalized;
}

216
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp Normal file
View File

@ -0,0 +1,216 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocation.hpp
*
* Interface for Control Allocation Algorithms
*
* Implementers of this interface are expected to update the members
* of this base class in the `allocate` method.
*
* Example usage:
* ```
* [...]
* // Initialization
* ControlAllocationMethodImpl alloc();
* alloc.setEffectivenessMatrix(effectiveness, actuator_trim);
* alloc.setActuatorMin(actuator_min);
* alloc.setActuatorMin(actuator_max);
*
* while (1) {
* [...]
*
* // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint
* alloc.setControlSetpoint(control_sp);
* alloc.allocate();
* actuator_sp = alloc.getActuatorSetpoint();
*
* // Check if the control setpoint was fully allocated
* unallocated_control = control_sp - alloc.getAllocatedControl()
*
* [...]
* }
* ```
*
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_actuator_setpoint.h>
class ControlAllocation
{
public:
ControlAllocation() = default;
virtual ~ControlAllocation() = default;
static constexpr uint8_t NUM_ACTUATORS = 16;
static constexpr uint8_t NUM_AXES = 6;
typedef matrix::Vector<float, NUM_ACTUATORS> ActuatorVector;
enum ControlAxis {
ROLL = 0,
PITCH,
YAW,
THRUST_X,
THRUST_Y,
THRUST_Z
};
/**
* Allocate control setpoint to actuators
*
* @param control_setpoint Desired control setpoint vector (input)
*/
virtual void allocate() = 0;
/**
* Set the control effectiveness matrix
*
* @param B Effectiveness matrix
*/
virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim);
/**
* Get the allocated actuator vector
*
* @return Actuator vector
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorSetpoint() const;
/**
* Set the desired control vector
*
* @param Control vector
*/
void setControlSetpoint(const matrix::Vector<float, NUM_AXES> &control);
/**
* Set the desired control vector
*
* @param Control vector
*/
const matrix::Vector<float, NUM_AXES> &getControlSetpoint() const;
/**
* Get the allocated control vector
*
* @return Control vector
*/
const matrix::Vector<float, NUM_AXES> &getAllocatedControl() const;
/**
* Get the control effectiveness matrix
*
* @return Effectiveness matrix
*/
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &getEffectivenessMatrix() const;
/**
* Set the minimum actuator values
*
* @param actuator_min Minimum actuator values
*/
void setActuatorMin(const matrix::Vector<float, NUM_ACTUATORS> &actuator_min);
/**
* Get the minimum actuator values
*
* @return Minimum actuator values
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMin() const;
/**
* Set the maximum actuator values
*
* @param actuator_max Maximum actuator values
*/
void setActuatorMax(const matrix::Vector<float, NUM_ACTUATORS> &actuator_max);
/**
* Get the maximum actuator values
*
* @return Maximum actuator values
*/
const matrix::Vector<float, NUM_ACTUATORS> &getActuatorMax() const;
/**
* Set the current actuator setpoint.
*
* Use this when a new allocation method is started to initialize it properly.
* In most cases, it is not needed to call this method before `allocate()`.
* Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`.
*
* @param actuator_sp Actuator setpoint
*/
void setActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp);
/**
* Clip the actuator setpoint between minimum and maximum values.
*
* The output is in the range [min; max]
*
* @param actuator Actuator vector to clip
*
* @return Clipped actuator setpoint
*/
matrix::Vector<float, NUM_ACTUATORS> clipActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator) const;
/**
* Normalize the actuator setpoint between minimum and maximum values.
*
* The output is in the range [-1; +1]
*
* @param actuator Actuator vector to normalize
*
* @return Clipped actuator setpoint
*/
matrix::Vector<float, NUM_ACTUATORS> normalizeActuatorSetpoint(const matrix::Vector<float, NUM_ACTUATORS> &actuator)
const;
protected:
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; //< Effectiveness matrix
matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; //< Neutral actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; //< Minimum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; //< Maximum actuator values
matrix::Vector<float, NUM_ACTUATORS> _actuator_sp; //< Actuator setpoint
matrix::Vector<float, NUM_AXES> _control_sp; //< Control setpoint
matrix::Vector<float, NUM_AXES> _control_allocated; //< Allocated control
matrix::Vector<float, NUM_AXES> _control_trim; //< Control at trim actuator values
};

76
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocationPseudoInverse.hpp
*
* Simple Control Allocation Algorithm
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ControlAllocationPseudoInverse.hpp"
void
ControlAllocationPseudoInverse::setEffectivenessMatrix(
const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator_trim)
{
ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim);
_mix_update_needed = true;
}
void
ControlAllocationPseudoInverse::updatePseudoInverse()
{
if (_mix_update_needed) {
_mix = matrix::geninv(_effectiveness);
_mix_update_needed = false;
}
}
void
ControlAllocationPseudoInverse::allocate()
{
//Compute new gains if needed
updatePseudoInverse();
// Allocate
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
// Clip
_actuator_sp = clipActuatorSetpoint(_actuator_sp);
// Compute achieved control
_control_allocated = _effectiveness * _actuator_sp;
}

70
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp Normal file
View File

@ -0,0 +1,70 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocationPseudoInverse.hpp
*
* Simple Control Allocation Algorithm
*
* It computes the pseudo-inverse of the effectiveness matrix
* Actuator saturation is handled by simple clipping, do not
* expect good performance in case of actuator saturation.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include "ControlAllocation.hpp"
class ControlAllocationPseudoInverse: public ControlAllocation
{
public:
ControlAllocationPseudoInverse() = default;
virtual ~ControlAllocationPseudoInverse() = default;
virtual void allocate() override;
virtual void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_trim) override;
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
bool _mix_update_needed{false};
/**
* Recalculate pseudo inverse if required.
*
*/
void updatePseudoInverse();
};

67
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp Normal file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocationTest.cpp
*
* Tests for Control Allocation Algorithms
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include <gtest/gtest.h>
#include <ControlAllocationPseudoInverse.hpp>
using namespace matrix;
TEST(ControlAllocationTest, AllZeroCase)
{
ControlAllocationPseudoInverse method;
matrix::Vector<float, 6> control_sp;
matrix::Vector<float, 6> control_allocated;
matrix::Vector<float, 6> control_allocated_expected;
matrix::Matrix<float, 6, 16> effectiveness;
matrix::Vector<float, 16> actuator_sp;
matrix::Vector<float, 16> actuator_trim;
matrix::Vector<float, 16> actuator_sp_expected;
method.setEffectivenessMatrix(effectiveness, actuator_trim, 16);
method.setControlSetpoint(control_sp);
method.allocate();
actuator_sp = method.getActuatorSetpoint();
control_allocated_expected = method.getAllocatedControl();
EXPECT_EQ(actuator_sp, actuator_sp_expected);
EXPECT_EQ(control_allocated, control_allocated_expected);
}

124
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp Normal file
View File

@ -0,0 +1,124 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocationSequentialDesaturation.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include "ControlAllocationSequentialDesaturation.hpp"
void
ControlAllocationSequentialDesaturation::allocate()
{
//Compute new gains if needed
updatePseudoInverse();
// Allocate
_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
// go through control axes from lowest to highest priority and unsaturate the actuators
for (unsigned i = 0; i < NUM_AXES; i++) {
desaturateActuators(_actuator_sp, _axis_prio_increasing[i]);
}
// Clip
_actuator_sp = clipActuatorSetpoint(_actuator_sp);
// Compute achieved control
_control_allocated = _effectiveness * _actuator_sp;
}
void ControlAllocationSequentialDesaturation::desaturateActuators(
ActuatorVector &actuator_sp,
const ControlAxis &axis)
{
ActuatorVector desaturation_vector = getDesaturationVector(axis);
float gain = computeDesaturationGain(desaturation_vector, actuator_sp);
actuator_sp = actuator_sp + gain * desaturation_vector;
gain = computeDesaturationGain(desaturation_vector, actuator_sp);
actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector;
}
ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(
const ControlAxis &axis)
{
ActuatorVector ret;
for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
ret(i) = _mix(i, axis);
}
return ret;
}
float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector,
const ActuatorVector &actuator_sp)
{
float k_min = 0.f;
float k_max = 0.f;
for (unsigned i = 0; i < NUM_ACTUATORS; i++) {
// Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway
if (fabsf(desaturation_vector(i)) < FLT_EPSILON) {
continue;
}
if (actuator_sp(i) < _actuator_min(i)) {
float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i);
if (k < k_min) { k_min = k; }
if (k > k_max) { k_max = k; }
}
if (actuator_sp(i) > _actuator_max(i)) {
float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i);
if (k < k_min) { k_min = k; }
if (k > k_max) { k_max = k; }
}
}
// Reduce the saturation as much as possible
return k_min + k_max;
}

89
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp Normal file
View File

@ -0,0 +1,89 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file ControlAllocationSequentialDesaturation.hpp
*
* Control Allocation Algorithm which sequentially modifies control demands in order to
* eliminate the saturation of the actuator setpoint vector.
*
*
* @author Roman Bapst <bapstroman@gmail.com>
*/
#pragma once
#include "ControlAllocationPseudoInverse.hpp"
class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse
{
public:
ControlAllocationSequentialDesaturation() = default;
virtual ~ControlAllocationSequentialDesaturation() = default;
void allocate() override;
private:
/**
* List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially
* go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate
* the actuator vector.
*/
ControlAxis _axis_prio_increasing [NUM_AXES] = {ControlAxis::YAW, ControlAxis::THRUST_Y, ControlAxis::THRUST_X, ControlAxis::THRUST_Z, ControlAxis::PITCH, ControlAxis::ROLL};
/**
* Desaturate actuator setpoint vector.
*
* @return Desaturated actuator setpoint vector.
*/
void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis);
/**
* Get desaturation vector.
*
* @param axis Control axis
* @return ActuatorVector Column of the pseudo-inverse matrix corresponding to the given control axis.
*/
ActuatorVector getDesaturationVector(const ControlAxis &axis);
/**
* Compute desaturation gain.
*
* @param desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis.
* @param Actuator setpoint vector.
* @return Gain which eliminates the saturation of the highest saturated actuator.
*/
float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp);
};

604
src/modules/control_allocator/ControlAllocator.cpp Normal file
View File

@ -0,0 +1,604 @@
/****************************************************************************
*
* 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 ControlAllocator.cpp
*
* Control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#include "ControlAllocator.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;
ControlAllocator::ControlAllocator() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
parameters_updated();
}
ControlAllocator::~ControlAllocator()
{
if (_control_allocation != nullptr) {
free(_control_allocation);
}
if (_actuator_effectiveness != nullptr) {
free(_actuator_effectiveness);
}
perf_free(_loop_perf);
}
bool
ControlAllocator::init()
{
if (!_vehicle_torque_setpoint_sub.registerCallback()) {
PX4_ERR("vehicle_torque_setpoint callback registration failed!");
return false;
}
if (!_vehicle_thrust_setpoint_sub.registerCallback()) {
PX4_ERR("vehicle_thrust_setpoint callback registration failed!");
return false;
}
return true;
}
void
ControlAllocator::parameters_updated()
{
// Allocation method & effectiveness source
// Do this first: in case a new method is loaded, it will be configured below
update_effectiveness_source();
update_allocation_method();
// Minimum actuator values
matrix::Vector<float, NUM_ACTUATORS> actuator_min;
actuator_min(0) = _param_ca_act0_min.get();
actuator_min(1) = _param_ca_act1_min.get();
actuator_min(2) = _param_ca_act2_min.get();
actuator_min(3) = _param_ca_act3_min.get();
actuator_min(4) = _param_ca_act4_min.get();
actuator_min(5) = _param_ca_act5_min.get();
actuator_min(6) = _param_ca_act6_min.get();
actuator_min(7) = _param_ca_act7_min.get();
actuator_min(8) = _param_ca_act8_min.get();
actuator_min(9) = _param_ca_act9_min.get();
actuator_min(10) = _param_ca_act10_min.get();
actuator_min(11) = _param_ca_act11_min.get();
actuator_min(12) = _param_ca_act12_min.get();
actuator_min(13) = _param_ca_act13_min.get();
actuator_min(14) = _param_ca_act14_min.get();
actuator_min(15) = _param_ca_act15_min.get();
_control_allocation->setActuatorMin(actuator_min);
// Maximum actuator values
matrix::Vector<float, NUM_ACTUATORS> actuator_max;
actuator_max(0) = _param_ca_act0_max.get();
actuator_max(1) = _param_ca_act1_max.get();
actuator_max(2) = _param_ca_act2_max.get();
actuator_max(3) = _param_ca_act3_max.get();
actuator_max(4) = _param_ca_act4_max.get();
actuator_max(5) = _param_ca_act5_max.get();
actuator_max(6) = _param_ca_act6_max.get();
actuator_max(7) = _param_ca_act7_max.get();
actuator_max(8) = _param_ca_act8_max.get();
actuator_max(9) = _param_ca_act9_max.get();
actuator_max(10) = _param_ca_act10_max.get();
actuator_max(11) = _param_ca_act11_max.get();
actuator_max(12) = _param_ca_act12_max.get();
actuator_max(13) = _param_ca_act13_max.get();
actuator_max(14) = _param_ca_act14_max.get();
actuator_max(15) = _param_ca_act15_max.get();
_control_allocation->setActuatorMax(actuator_max);
// Get actuator effectiveness and trim
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
// Assign control effectiveness matrix
_control_allocation->setEffectivenessMatrix(effectiveness, trim);
}
void
ControlAllocator::update_allocation_method()
{
AllocationMethod method = (AllocationMethod)_param_ca_method.get();
if (_allocation_method_id != method) {
// Save current state
matrix::Vector<float, NUM_ACTUATORS> actuator_sp;
if (_control_allocation != nullptr) {
actuator_sp = _control_allocation->getActuatorSetpoint();
}
// try to instanciate new allocation method
ControlAllocation *tmp = nullptr;
switch (method) {
case AllocationMethod::PSEUDO_INVERSE:
tmp = new ControlAllocationPseudoInverse();
break;
case AllocationMethod::SEQUENTIAL_DESATURATION:
tmp = new ControlAllocationSequentialDesaturation();
break;
default:
PX4_ERR("Unknown allocation method");
tmp = nullptr;
break;
}
// Replace previous method with new one
if (tmp == nullptr) {
// It did not work, forget about it
PX4_ERR("Control allocation init failed");
_param_ca_method.set((int)_allocation_method_id);
} else if (_control_allocation == tmp) {
// Nothing has changed, this should not happen
PX4_ERR("Control allocation init error");
_allocation_method_id = method;
} else {
// Successful update
PX4_INFO("Control allocation init success");
// Swap allocation methods
if (_control_allocation != nullptr) {
free(_control_allocation);
}
_control_allocation = tmp;
// Save method id
_allocation_method_id = method;
// Configure new allocation method
_control_allocation->setActuatorSetpoint(actuator_sp);
}
}
// Guard against bad initialization
if (_control_allocation == nullptr) {
PX4_ERR("Falling back to ControlAllocationPseudoInverse");
_control_allocation = new ControlAllocationPseudoInverse();
_allocation_method_id = AllocationMethod::PSEUDO_INVERSE;
}
}
void
ControlAllocator::update_effectiveness_source()
{
EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get();
if (_effectiveness_source_id != source) {
// try to instanciate new effectiveness source
ActuatorEffectiveness *tmp = nullptr;
switch (source) {
case EffectivenessSource::NONE:
case EffectivenessSource::MULTIROTOR:
tmp = new ActuatorEffectivenessMultirotor();
break;
case EffectivenessSource::STANDARD_VTOL:
tmp = new ActuatorEffectivenessStandardVTOL();
break;
case EffectivenessSource::TILTROTOR_VTOL:
tmp = new ActuatorEffectivenessTiltrotorVTOL();
break;
default:
PX4_ERR("Unknown airframe");
tmp = nullptr;
break;
}
// Replace previous source with new one
if (tmp == nullptr) {
// It did not work, forget about it
PX4_ERR("Actuator effectiveness init failed");
_param_ca_airframe.set((int)_effectiveness_source_id);
} else if (_actuator_effectiveness == tmp) {
// Nothing has changed, this should not happen
PX4_ERR("Actuator effectiveness init error");
_effectiveness_source_id = source;
} else {
// Successful update
PX4_INFO("Actuator effectiveness init success");
// Swap effectiveness sources
if (_actuator_effectiveness != nullptr) {
free(_actuator_effectiveness);
}
_actuator_effectiveness = tmp;
// Save source id
_effectiveness_source_id = source;
}
}
// Guard against bad initialization
if (_actuator_effectiveness == nullptr) {
PX4_ERR("Falling back to ActuatorEffectivenessMultirotor");
_actuator_effectiveness = new ActuatorEffectivenessMultirotor();
_effectiveness_source_id = EffectivenessSource::MULTIROTOR;
}
}
void
ControlAllocator::Run()
{
if (should_exit()) {
_vehicle_torque_setpoint_sub.unregisterCallback();
_vehicle_thrust_setpoint_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(&param_update);
updateParams();
parameters_updated();
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status = {};
_vehicle_status_sub.update(&vehicle_status);
ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
// Check if the current flight phase is HOVER or FIXED_WING
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT;
} else {
flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
}
// Special cases for VTOL in transition
if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) {
if (vehicle_status.in_transition_to_fw) {
flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF;
} else {
flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF;
}
}
// Forward to effectiveness source
_actuator_effectiveness->setFlightPhase(flight_phase);
}
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
_last_run = now;
bool do_update = false;
vehicle_torque_setpoint_s vehicle_torque_setpoint;
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
// Run allocator on torque changes
if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) {
_torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz);
do_update = true;
_timestamp_sample = vehicle_torque_setpoint.timestamp_sample;
}
// Also run allocator on thrust setpoint changes if the torque setpoint
// has not been updated for more than 5ms
if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) {
_thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
if (dt > 5_ms) {
do_update = true;
_timestamp_sample = vehicle_thrust_setpoint.timestamp_sample;
}
}
if (do_update) {
// Update effectiveness matrix if needed
if (_actuator_effectiveness->update()) {
matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> effectiveness = _actuator_effectiveness->getEffectivenessMatrix();
matrix::Vector<float, NUM_ACTUATORS> trim = _actuator_effectiveness->getActuatorTrim();
// Set 0 effectiveness for actuators that are disabled (act_min >= act_max)
matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
for (size_t j = 0; j < NUM_ACTUATORS; j++) {
if (actuator_min(j) >= actuator_max(j)) {
for (size_t i = 0; i < NUM_AXES; i++) {
effectiveness(i, j) = 0.0f;
}
}
}
// Assign control effectiveness matrix
if ((effectiveness - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) {
_control_allocation->setEffectivenessMatrix(effectiveness, trim);
}
}
// Set control setpoint vector
matrix::Vector<float, NUM_AXES> c;
c(0) = _torque_sp(0);
c(1) = _torque_sp(1);
c(2) = _torque_sp(2);
c(3) = _thrust_sp(0);
c(4) = _thrust_sp(1);
c(5) = _thrust_sp(2);
_control_allocation->setControlSetpoint(c);
// Do allocation
_control_allocation->allocate();
// Publish actuator setpoint and allocator status
publish_actuator_setpoint();
publish_control_allocator_status();
// Publish on legacy topics for compatibility with
// the current mixer system and multicopter controller
// TODO: remove
publish_legacy_actuator_controls();
}
perf_end(_loop_perf);
}
void
ControlAllocator::publish_actuator_setpoint()
{
matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
vehicle_actuator_setpoint_s vehicle_actuator_setpoint{};
vehicle_actuator_setpoint.timestamp = hrt_absolute_time();
vehicle_actuator_setpoint.timestamp_sample = _timestamp_sample;
actuator_sp.copyTo(vehicle_actuator_setpoint.actuator);
_vehicle_actuator_setpoint_pub.publish(vehicle_actuator_setpoint);
}
void
ControlAllocator::publish_control_allocator_status()
{
control_allocator_status_s control_allocator_status{};
control_allocator_status.timestamp = hrt_absolute_time();
// Allocated control
matrix::Vector<float, NUM_AXES> allocated_control = _control_allocation->getAllocatedControl();
control_allocator_status.allocated_torque[0] = allocated_control(0);
control_allocator_status.allocated_torque[1] = allocated_control(1);
control_allocator_status.allocated_torque[2] = allocated_control(2);
control_allocator_status.allocated_thrust[0] = allocated_control(3);
control_allocator_status.allocated_thrust[1] = allocated_control(4);
control_allocator_status.allocated_thrust[2] = allocated_control(5);
// Unallocated control
matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation->getControlSetpoint() - allocated_control;
control_allocator_status.unallocated_torque[0] = unallocated_control(0);
control_allocator_status.unallocated_torque[1] = unallocated_control(1);
control_allocator_status.unallocated_torque[2] = unallocated_control(2);
control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
unallocated_control(2)).norm() < FLT_EPSILON);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
unallocated_control(5)).norm() < FLT_EPSILON);
// Actuator saturation
matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
matrix::Vector<float, NUM_ACTUATORS> actuator_min = _control_allocation->getActuatorMin();
matrix::Vector<float, NUM_ACTUATORS> actuator_max = _control_allocation->getActuatorMax();
for (size_t i = 0; i < NUM_ACTUATORS; i++) {
if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {
control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER;
} else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) {
control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER;
}
}
_control_allocator_status_pub.publish(control_allocator_status);
}
void
ControlAllocator::publish_legacy_actuator_controls()
{
// For compatibility with the current mixer system,
// publish normalized version on actuator_controls_4/5
actuator_controls_s actuator_controls_4{};
actuator_controls_s actuator_controls_5{};
actuator_controls_4.timestamp = hrt_absolute_time();
actuator_controls_5.timestamp = hrt_absolute_time();
actuator_controls_4.timestamp_sample = _timestamp_sample;
actuator_controls_5.timestamp_sample = _timestamp_sample;
matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
actuator_sp);
for (size_t i = 0; i < 8; i++) {
actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f;
actuator_controls_5.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i + 8))) ? actuator_sp_normalized(i + 8) : 0.0f;
}
_actuator_controls_4_pub.publish(actuator_controls_4);
_actuator_controls_5_pub.publish(actuator_controls_5);
}
int ControlAllocator::task_spawn(int argc, char *argv[])
{
ControlAllocator *instance = new ControlAllocator();
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 ControlAllocator::print_status()
{
PX4_INFO("Running");
// Print current allocation method
switch (_allocation_method_id) {
case AllocationMethod::NONE:
PX4_INFO("Method: None");
break;
case AllocationMethod::PSEUDO_INVERSE:
PX4_INFO("Method: Pseudo-inverse");
break;
case AllocationMethod::SEQUENTIAL_DESATURATION:
PX4_INFO("Method: Sequential desaturation");
break;
}
// Print current airframe
switch ((EffectivenessSource)_param_ca_airframe.get()) {
case EffectivenessSource::NONE:
PX4_INFO("EffectivenessSource: None");
break;
case EffectivenessSource::MULTIROTOR:
PX4_INFO("EffectivenessSource: MC parameters");
break;
case EffectivenessSource::STANDARD_VTOL:
PX4_INFO("EffectivenessSource: Standard VTOL");
break;
case EffectivenessSource::TILTROTOR_VTOL:
PX4_INFO("EffectivenessSource: Tiltrotor VTOL");
break;
}
// Print current effectiveness matrix
if (_control_allocation != nullptr) {
const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness = _control_allocation->getEffectivenessMatrix();
PX4_INFO("Effectiveness.T =");
effectiveness.T().print();
}
// Print perf
perf_print_counter(_loop_perf);
return 0;
}
int ControlAllocator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int ControlAllocator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This implements control allocation. It takes torque and thrust setpoints
as inputs and outputs actuator setpoint messages.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
/**
* Control Allocator app start / stop handling function
*/
extern "C" __EXPORT int control_allocator_main(int argc, char *argv[]);
int control_allocator_main(int argc, char *argv[])
{
return ControlAllocator::main(argc, argv);
}

201
src/modules/control_allocator/ControlAllocator.hpp Normal file
View File

@ -0,0 +1,201 @@
/****************************************************************************
*
* 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 ControlAllocator.hpp
*
* Control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
#pragma once
#include <ActuatorEffectiveness.hpp>
#include <ActuatorEffectivenessMultirotor.hpp>
#include <ActuatorEffectivenessStandardVTOL.hpp>
#include <ActuatorEffectivenessTiltrotorVTOL.hpp>
#include <ControlAllocation.hpp>
#include <ControlAllocationPseudoInverse.hpp>
#include <ControlAllocationSequentialDesaturation.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_actuator_setpoint.h>
#include <uORB/topics/vehicle_status.h>
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::WorkItem
{
public:
ControlAllocator();
virtual ~ControlAllocator();
/** @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();
static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS;
static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES;
private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void update_allocation_method();
void update_effectiveness_source();
void publish_actuator_setpoint();
void publish_control_allocator_status();
void publish_legacy_actuator_controls();
void publish_legacy_multirotor_motor_limits();
enum class AllocationMethod {
NONE = -1,
PSEUDO_INVERSE = 0,
SEQUENTIAL_DESATURATION = 1,
};
AllocationMethod _allocation_method_id{AllocationMethod::NONE};
ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations
enum class EffectivenessSource {
NONE = -1,
MULTIROTOR = 0,
STANDARD_VTOL = 1,
TILTROTOR_VTOL = 2,
};
EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE};
ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness
// Inputs
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */
// Outputs
uORB::Publication<vehicle_actuator_setpoint_s> _vehicle_actuator_setpoint_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */
uORB::Publication<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */
// actuator_controls publication handles (temporary hack to plug actuator_setpoint into the mixer system)
uORB::Publication<actuator_controls_s> _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */
uORB::Publication<actuator_controls_s> _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
matrix::Vector3f _torque_sp;
matrix::Vector3f _thrust_sp;
// float _battery_scale_factor{1.0f};
// float _airspeed_scale_factor{1.0f};
perf_counter_t _loop_perf; /**< loop duration performance counter */
hrt_abstime _task_start{hrt_absolute_time()};
hrt_abstime _last_run{0};
hrt_abstime _timestamp_sample{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
(ParamBool<px4::params::CA_BAT_SCALE_EN>) _param_ca_bat_scale_en,
(ParamBool<px4::params::CA_AIR_SCALE_EN>) _param_ca_air_scale_en,
(ParamFloat<px4::params::CA_ACT0_MIN>) _param_ca_act0_min,
(ParamFloat<px4::params::CA_ACT1_MIN>) _param_ca_act1_min,
(ParamFloat<px4::params::CA_ACT2_MIN>) _param_ca_act2_min,
(ParamFloat<px4::params::CA_ACT3_MIN>) _param_ca_act3_min,
(ParamFloat<px4::params::CA_ACT4_MIN>) _param_ca_act4_min,
(ParamFloat<px4::params::CA_ACT5_MIN>) _param_ca_act5_min,
(ParamFloat<px4::params::CA_ACT6_MIN>) _param_ca_act6_min,
(ParamFloat<px4::params::CA_ACT7_MIN>) _param_ca_act7_min,
(ParamFloat<px4::params::CA_ACT8_MIN>) _param_ca_act8_min,
(ParamFloat<px4::params::CA_ACT9_MIN>) _param_ca_act9_min,
(ParamFloat<px4::params::CA_ACT10_MIN>) _param_ca_act10_min,
(ParamFloat<px4::params::CA_ACT11_MIN>) _param_ca_act11_min,
(ParamFloat<px4::params::CA_ACT12_MIN>) _param_ca_act12_min,
(ParamFloat<px4::params::CA_ACT13_MIN>) _param_ca_act13_min,
(ParamFloat<px4::params::CA_ACT14_MIN>) _param_ca_act14_min,
(ParamFloat<px4::params::CA_ACT15_MIN>) _param_ca_act15_min,
(ParamFloat<px4::params::CA_ACT0_MAX>) _param_ca_act0_max,
(ParamFloat<px4::params::CA_ACT1_MAX>) _param_ca_act1_max,
(ParamFloat<px4::params::CA_ACT2_MAX>) _param_ca_act2_max,
(ParamFloat<px4::params::CA_ACT3_MAX>) _param_ca_act3_max,
(ParamFloat<px4::params::CA_ACT4_MAX>) _param_ca_act4_max,
(ParamFloat<px4::params::CA_ACT5_MAX>) _param_ca_act5_max,
(ParamFloat<px4::params::CA_ACT6_MAX>) _param_ca_act6_max,
(ParamFloat<px4::params::CA_ACT7_MAX>) _param_ca_act7_max,
(ParamFloat<px4::params::CA_ACT8_MAX>) _param_ca_act8_max,
(ParamFloat<px4::params::CA_ACT9_MAX>) _param_ca_act9_max,
(ParamFloat<px4::params::CA_ACT10_MAX>) _param_ca_act10_max,
(ParamFloat<px4::params::CA_ACT11_MAX>) _param_ca_act11_max,
(ParamFloat<px4::params::CA_ACT12_MAX>) _param_ca_act12_max,
(ParamFloat<px4::params::CA_ACT13_MAX>) _param_ca_act13_max,
(ParamFloat<px4::params::CA_ACT14_MAX>) _param_ca_act14_max,
(ParamFloat<px4::params::CA_ACT15_MAX>) _param_ca_act15_max
)
};

314
src/modules/control_allocator/control_allocator_params.c Normal file
View File

@ -0,0 +1,314 @@
/****************************************************************************
*
* 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 control_allocator_params.c
*
* Parameters for the control allocator.
*
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
*/
/**
* Airframe ID
*
* This is used to retrieve pre-computed control effectiveness matrix
*
* @min 0
* @max 2
* @value 0 Multirotor
* @value 1 Standard VTOL (WIP)
* @value 2 Tiltrotor VTOL (WIP)
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIRFRAME, 0);
/**
* Control allocation method
*
* @value 0 Pseudo-inverse with output clipping (default)
* @value 1 Pseudo-inverse with sequential desaturation technique
* @min 0
* @max 1
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_METHOD, 0);
/**
* 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 Control Allocation
*/
PARAM_DEFINE_INT32(CA_BAT_SCALE_EN, 0);
/**
* Airspeed scaler
*
* This compensates for the variation of flap effectiveness with airspeed.
*
* @boolean
* @group Control Allocation
*/
PARAM_DEFINE_INT32(CA_AIR_SCALE_EN, 0);
/**
* Minimum value for actuator 0
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT0_MIN, 0.0);
/**
* Minimum value for actuator 1
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT1_MIN, 0.0);
/**
* Minimum value for actuator 2
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT2_MIN, 0.0);
/**
* Minimum value for actuator 3
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT3_MIN, 0.0);
/**
* Minimum value for actuator 4
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT4_MIN, 0.0);
/**
* Minimum value for actuator 5
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT5_MIN, 0.0);
/**
* Minimum value for actuator 6
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT6_MIN, 0.0);
/**
* Minimum value for actuator 7
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT7_MIN, 0.0);
/**
* Minimum value for actuator 8
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT8_MIN, 0.0);
/**
* Minimum value for actuator 9
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT9_MIN, 0.0);
/**
* Minimum value for actuator 10
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT10_MIN, 0.0);
/**
* Minimum value for actuator 11
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT11_MIN, 0.0);
/**
* Minimum value for actuator 12
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT12_MIN, 0.0);
/**
* Minimum value for actuator 13
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT13_MIN, 0.0);
/**
* Minimum value for actuator 14
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT14_MIN, 0.0);
/**
* Minimum value for actuator 15
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT15_MIN, 0.0);
/**
* Maximum value for actuator 0
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT0_MAX, 0.0);
/**
* Maximum value for actuator 1
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT1_MAX, 0.0);
/**
* Maximum value for actuator 2
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT2_MAX, 0.0);
/**
* Maximum value for actuator 3
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT3_MAX, 0.0);
/**
* Maximum value for actuator 4
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT4_MAX, 0.0);
/**
* Maximum value for actuator 5
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT5_MAX, 0.0);
/**
* Maximum value for actuator 6
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT6_MAX, 0.0);
/**
* Maximum value for actuator 7
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT7_MAX, 0.0);
/**
* Maximum value for actuator 8
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT8_MAX, 0.0);
/**
* Maximum value for actuator 9
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT9_MAX, 0.0);
/**
* Maximum value for actuator 10
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT10_MAX, 0.0);
/**
* Maximum value for actuator 11
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT11_MAX, 0.0);
/**
* Maximum value for actuator 12
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT12_MAX, 0.0);
/**
* Maximum value for actuator 13
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT13_MAX, 0.0);
/**
* Maximum value for actuator 14
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0);
/**
* Maximum value for actuator 15
*
* @group Control Allocation
*/
PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0);

14
src/modules/logger/logged_topics.cpp
View File

@ -48,6 +48,10 @@ void LoggedTopics::add_default_topics()
add_topic("actuator_armed");
add_topic("actuator_controls_0", 50);
add_topic("actuator_controls_1", 100);
add_topic("actuator_controls_2", 100);
add_topic("actuator_controls_3", 100);
add_topic("actuator_controls_4", 100);
add_topic("actuator_controls_5", 100);
add_topic("airspeed", 1000);
add_topic("airspeed_validated", 200);
add_topic("camera_capture");
@ -106,6 +110,13 @@ void LoggedTopics::add_default_topics()
add_topic("vehicle_status_flags");
add_topic("vtol_vehicle_status", 200);
// Control allocaton topics
add_topic("vehicle_angular_acceleration_setpoint", 20);
add_topic("vehicle_angular_acceleration", 20);
add_topic("vehicle_thrust_setpoint", 20);
add_topic("vehicle_torque_setpoint", 20);
add_topic("vehicle_actuator_setpoint", 20);
// multi topics
add_topic_multi("actuator_outputs", 100, 2);
add_topic_multi("logger_status", 0, 2);
@ -251,6 +262,9 @@ void LoggedTopics::add_system_identification_topics()
add_topic("actuator_controls_0");
add_topic("actuator_controls_1");
add_topic("sensor_combined");
add_topic("vehicle_angular_acceleration");
add_topic("vehicle_angular_acceleration_setpoint");
add_topic("vehicle_torque_setpoint");
}
int LoggedTopics::add_topics_from_file(const char *fname)