diff --git a/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc new file mode 100644 index 000000000000..905bba773554 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc @@ -0,0 +1,53 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor SITL +# +# @type Quadrotor Wide +# +# @maintainer Julian Oes +# + +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 + diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 0de4a44facd4..711e6f34d835 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -41,9 +41,31 @@ then param set VT_TYPE 2 param set VT_B_TRANS_DUR 8 + param set CA_AIRFRAME 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_ACT4_MIN 0.0 + param set CA_ACT4_MAX 1.0 + + param set CA_ACT5_MIN -1.0 + param set CA_ACT5_MAX 1.0 + param set CA_ACT6_MIN -1.0 + param set CA_ACT6_MAX 1.0 + param set CA_ACT7_MIN -1.0 + param set CA_ACT7_MAX 1.0 + fi set MAV_TYPE 22 -set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix -set MIXER custom +# set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix +# set MIXER custom +set MIXER direct diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index dd64031f9222..c111fa2828ec 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -41,12 +41,35 @@ then param set VT_TILT_FW 1 param set VT_TILT_TRANS 0.6 param set VT_ELEV_MC_LOCK 0 - param set VT_TYPE 1 param set VT_B_TRANS_DUR 8 + # param set VT_TYPE 1 + param set VT_TYPE 2 + + param set CA_AIRFRAME 2 + + param set CA_ACT0_MIN 0 + param set CA_ACT0_MAX 1 + param set CA_ACT1_MIN 0 + param set CA_ACT1_MAX 1 + param set CA_ACT2_MIN 0 + param set CA_ACT2_MAX 1 + param set CA_ACT3_MIN 0 + param set CA_ACT3_MAX 1 + + param set CA_ACT4_MIN -0.1 + param set CA_ACT4_MAX 1.5 + param set CA_ACT5_MIN -0.1 + param set CA_ACT5_MAX 1.5 + param set CA_ACT6_MIN -0.1 + param set CA_ACT6_MAX 1.5 + param set CA_ACT7_MIN -0.1 + param set CA_ACT7_MAX 1.5 + fi set MAV_TYPE 21 -set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix +# set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix +set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl_direct.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc new file mode 100644 index 000000000000..37139b325278 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post new file mode 100644 index 000000000000..80264373dc41 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 4446c6a658bc..557a2eee1bb1 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -51,4 +51,5 @@ px4_add_romfs_files( rc.vehicle_setup rc.vtol_apps rc.vtol_defaults + rc.ctrlalloc ) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x index 709e8197ef5a..70be5a3f433a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -23,5 +23,6 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x +# set MIXER quad set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc new file mode 100644 index 000000000000..0ce32d4489fc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc new file mode 100644 index 000000000000..b62c06f45dda --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc new file mode 100644 index 000000000000..e94b5fb16b59 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c565aebf460c..8f3dedd01613 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,7 @@ ekf2 start fw_att_control start fw_pos_control_l1 start airspeed_selector start + # # Start Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 40a1e3df23e1..2a12d878aafc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -27,6 +27,7 @@ mc_hover_thrust_estimator start fw_att_control start vtol fw_pos_control_l1 start vtol +# # Start Land Detector # Multicopter for now until we have something for VTOL # diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt index 18d536981b36..8f52ef94bd46 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt @@ -39,4 +39,5 @@ px4_add_romfs_files( standard_vtol_sitl.main.mix tiltrotor_sitl.main.mix uuv_x_sitl.main.mix + tiltrotor_sitl_direct.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix new file mode 100644 index 000000000000..ea3adaa0b77d --- /dev/null +++ b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 9a4e28eb9217..1238ae2171fd 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -44,6 +44,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 diff --git a/ROMFS/px4fmu_common/mixers/direct.main.mix b/ROMFS/px4fmu_common/mixers/direct.main.mix new file mode 100644 index 000000000000..65194e94b71c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/direct.main.mix @@ -0,0 +1,10 @@ +# Direct mixer + +A: 0 +A: 1 +A: 2 +A: 3 +A: 4 +A: 5 +A: 6 +A: 7 diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 82e7c452815a..c5688986b3a7 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -96,7 +96,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) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 1af7e29dbb1e..e1cc6b0f5187 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 1af7e29dbb1ecce7b0b191c9deb24ab1f13916ab +Subproject commit e1cc6b0f51875b624eb08063b6d83d765d2ddea0 diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake new file mode 100644 index 000000000000..b9fae5dd19f5 --- /dev/null +++ b/boards/px4/fmu-v3/ctrlalloc.cmake @@ -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 + ) diff --git a/boards/px4/fmu-v4/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake new file mode 100644 index 000000000000..7b49a7f0986f --- /dev/null +++ b/boards/px4/fmu-v4/ctrlalloc.cmake @@ -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 + ) diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake new file mode 100644 index 000000000000..340716aa6113 --- /dev/null +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -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 + ) diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake new file mode 100644 index 000000000000..9fa612037fad --- /dev/null +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -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() diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index f36effca6bf8..186d0707b7a4 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -24,6 +24,7 @@ px4_add_board( attitude_estimator_q camera_feedback commander + control_allocator dataman ekf2 events diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a836c6c4df43..450677b02b30 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -48,6 +48,7 @@ set(msg_files collision_constraints.msg collision_report.msg commander_state.msg + control_allocator_status.msg cpuload.msg debug_array.msg debug_key_value.msg @@ -138,8 +139,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 @@ -160,6 +163,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 diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 25ff45f432ed..c4b52ea48dfe 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -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 diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg new file mode 100644 index 000000000000..b26d2d36b12c --- /dev/null +++ b/msg/control_allocator_status.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +bool 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 + +bool 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. diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index a0031db16a83..b201fe0707ca 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -295,6 +295,16 @@ rtps: id: 131 - msg: yaw_estimator_status id: 132 + - msg: vehicle_angular_acceleration_setpoint + id: 133 + - msg: vehicle_torque_setpoint + id: 134 + - msg: vehicle_thrust_setpoint + id: 135 + - msg: vehicle_actuator_setpoint + id: 136 + - msg: control_allocator_status + id: 137 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_actuator_setpoint.msg b/msg/vehicle_actuator_setpoint.msg new file mode 100644 index 000000000000..5d4781d46e1b --- /dev/null +++ b/msg/vehicle_actuator_setpoint.msg @@ -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 diff --git a/msg/vehicle_angular_acceleration_setpoint.msg b/msg/vehicle_angular_acceleration_setpoint.msg new file mode 100644 index 000000000000..69f5b0bae30f --- /dev/null +++ b/msg/vehicle_angular_acceleration_setpoint.msg @@ -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 diff --git a/msg/vehicle_thrust_setpoint.msg b/msg/vehicle_thrust_setpoint.msg new file mode 100644 index 000000000000..c84bd87a08b2 --- /dev/null +++ b/msg/vehicle_thrust_setpoint.msg @@ -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) diff --git a/msg/vehicle_torque_setpoint.msg b/msg/vehicle_torque_setpoint.msg new file mode 100644 index 000000000000..83b733dd831f --- /dev/null +++ b/msg/vehicle_torque_setpoint.msg @@ -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) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 949c19495aa8..ed630d624182 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -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}; diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 105a242d2a41..8d01fae857ea 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -77,7 +77,7 @@ ExternalProject_Add(flightgear_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_obs_avoid iris_rtps iris_ctrlalloc px4vision solo typhoon_h480 typhoon_ctrlalloc plane plane_cam plane_catapult plane_lidar standard_vtol tailsitter tiltrotor rover r1_rover boat diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp index 1619d4564db0..44927628ce38 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp @@ -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(); diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp index 643cff73aa39..ec8b0aa218ad 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp @@ -74,12 +74,16 @@ class LowPassFilter2pVector3f // 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}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp new file mode 100644 index 000000000000..9e03335c12d3 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp @@ -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 + */ + +#include "AllocatedActuatorMixer.hpp" + +#include +#include +#include + +// #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; +} diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp new file mode 100644 index 000000000000..2ee9b66e22a3 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp @@ -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 + */ + +#pragma once + +#include + +/** + * 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); +}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt new file mode 100644 index 000000000000..a9bd1cff6c88 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt @@ -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 Mixer) +add_dependencies(AllocatedActuatorMixer prebuild_targets) diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt index 716a99d66a19..61af1d9b6e7d 100644 --- a/src/lib/mixer/CMakeLists.txt +++ b/src/lib/mixer/CMakeLists.txt @@ -34,6 +34,7 @@ # required by other mixers add_subdirectory(Mixer) +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 diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index c9575301d959..f87eefd46a26 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -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; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index f5cb140e1fa6..620bbd1dd377 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -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), @@ -508,9 +510,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 */ @@ -520,9 +524,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; } diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp new file mode 100644 index 000000000000..7c3ffb206664 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp @@ -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 +#include + +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 &saturation_positive, + const matrix::Vector &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(); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp new file mode 100644 index 000000000000..aee955c93bc5 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp @@ -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 + +#include + +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 &saturation_positive, + const matrix::Vector &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()}; ///< inertia matrix + + // States + matrix::Vector3f _angular_velocity_int; + matrix::Vector _saturation_positive; + matrix::Vector _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 +}; diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp new file mode 100644 index 000000000000..9f4c85af469e --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp @@ -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 +#include + +using namespace matrix; + +TEST(AngularVelocityControlTest, AllZeroCase) +{ + AngularVelocityControl control; + control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); + Vector3f torque = control.getTorqueSetpoint(); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt new file mode 100644 index 000000000000..05bb7dd7b25c --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt @@ -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) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp new file mode 100644 index 000000000000..1d8dcba7c947 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -0,0 +1,342 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +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(¶m_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 + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!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); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp new file mode 100644 index 000000000000..a8fdd473e6f0 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class AngularVelocityController : public ModuleBase, 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_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ + uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */ + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */ + uORB::Publication _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) _param_avc_x_p, + (ParamFloat) _param_avc_x_i, + (ParamFloat) _param_avc_x_i_lim, + (ParamFloat) _param_avc_x_d, + (ParamFloat) _param_avc_x_ff, + (ParamFloat) _param_avc_x_k, + + (ParamFloat) _param_avc_y_p, + (ParamFloat) _param_avc_y_i, + (ParamFloat) _param_avc_y_i_lim, + (ParamFloat) _param_avc_y_d, + (ParamFloat) _param_avc_y_ff, + (ParamFloat) _param_avc_y_k, + + (ParamFloat) _param_avc_z_p, + (ParamFloat) _param_avc_z_i, + (ParamFloat) _param_avc_z_i_lim, + (ParamFloat) _param_avc_z_d, + (ParamFloat) _param_avc_z_ff, + (ParamFloat) _param_avc_z_k, + + (ParamFloat) _param_vm_mass, + (ParamFloat) _param_vm_inertia_xx, + (ParamFloat) _param_vm_inertia_yy, + (ParamFloat) _param_vm_inertia_zz, + (ParamFloat) _param_vm_inertia_xy, + (ParamFloat) _param_vm_inertia_xz, + (ParamFloat) _param_vm_inertia_yz, + + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte + ) + +}; + diff --git a/src/modules/angular_velocity_controller/CMakeLists.txt b/src/modules/angular_velocity_controller/CMakeLists.txt new file mode 100644 index 000000000000..4cfc369db7ba --- /dev/null +++ b/src/modules/angular_velocity_controller/CMakeLists.txt @@ -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 + ) diff --git a/src/modules/angular_velocity_controller/angular_velocity_controller_params.c b/src/modules/angular_velocity_controller/angular_velocity_controller_params.c new file mode 100644 index 000000000000..112ffdc765cc --- /dev/null +++ b/src/modules/angular_velocity_controller/angular_velocity_controller_params.c @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * 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 + * @author Anton Babushkin + * @author Julien Lecoeur + */ + +/** + * 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 N.m/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 N.m + * @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. + * + * @unit 1 + * @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 N.m.s/rad + * @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. + * + * @unit 1 + * @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 N.m/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 N.m + * @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. + * + * @unit 1 + * @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 N.m.s/rad + * @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. + * + * @unit 1 + * @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 N.m/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 N.m + * @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. + * + * @unit 1 + * @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 N.m.s/rad + * @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. + * + * @unit 1 + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f); diff --git a/src/modules/angular_velocity_controller/vehicle_model_params.c b/src/modules/angular_velocity_controller/vehicle_model_params.c new file mode 100644 index 000000000000..df133a8b5ceb --- /dev/null +++ b/src/modules/angular_velocity_controller/vehicle_model_params.c @@ -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 + */ + +/** + * 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); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp new file mode 100644 index 000000000000..bd6ea7af11ee --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include + +#include +#include + +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 + }; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + virtual void setFlightPhase(const FlightPhase &flight_phase) + { + _flight_phase = flight_phase; + } + + /** + * Get the control effectiveness matrix if updated + * + * @return true if updated and matrix is set + */ + virtual bool getEffectivenessMatrix(matrix::Matrix &matrix) = 0; + + /** + * Get the actuator trims + * + * @return Actuator trims + */ + const matrix::Vector &getActuatorTrim() const + { + return _trim; + } + + /** + * Get the current flight phase + * + * @return Flight phase + */ + const FlightPhase &getFlightPhase() const + { + return _flight_phase; + } + + /** + * Get the number of actuators + */ + virtual int numActuators() const = 0; + +protected: + matrix::Vector _trim; ///< Actuator trim + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp new file mode 100644 index 000000000000..11cab60aa070 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * 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 + */ + +#include "ActuatorEffectivenessMultirotor.hpp" + +ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): + ModuleParams(nullptr) +{ +} + +bool +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + // Check if parameters have changed + if (_updated || _parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + _updated = false; + + // 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(); + + _num_actuators = computeEffectivenessMatrix(geometry, matrix); + return true; + } + + return false; +} + +int +ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness) +{ + int num_actuators = 0; + + effectiveness.setZero(); + + 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; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // 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); + } + + num_actuators = i + 1; + } + + return num_actuators; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp new file mode 100644 index 000000000000..1bb4c9efc223 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMultirotor(); + virtual ~ActuatorEffectivenessMultirotor() = default; + + 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 int computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + int numActuators() const override { return _num_actuators; } +private: + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + + bool _updated{true}; + int _num_actuators{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_mc_r0_px, + (ParamFloat) _param_ca_mc_r0_py, + (ParamFloat) _param_ca_mc_r0_pz, + (ParamFloat) _param_ca_mc_r0_ax, + (ParamFloat) _param_ca_mc_r0_ay, + (ParamFloat) _param_ca_mc_r0_az, + (ParamFloat) _param_ca_mc_r0_ct, + (ParamFloat) _param_ca_mc_r0_km, + + (ParamFloat) _param_ca_mc_r1_px, + (ParamFloat) _param_ca_mc_r1_py, + (ParamFloat) _param_ca_mc_r1_pz, + (ParamFloat) _param_ca_mc_r1_ax, + (ParamFloat) _param_ca_mc_r1_ay, + (ParamFloat) _param_ca_mc_r1_az, + (ParamFloat) _param_ca_mc_r1_ct, + (ParamFloat) _param_ca_mc_r1_km, + + (ParamFloat) _param_ca_mc_r2_px, + (ParamFloat) _param_ca_mc_r2_py, + (ParamFloat) _param_ca_mc_r2_pz, + (ParamFloat) _param_ca_mc_r2_ax, + (ParamFloat) _param_ca_mc_r2_ay, + (ParamFloat) _param_ca_mc_r2_az, + (ParamFloat) _param_ca_mc_r2_ct, + (ParamFloat) _param_ca_mc_r2_km, + + (ParamFloat) _param_ca_mc_r3_px, + (ParamFloat) _param_ca_mc_r3_py, + (ParamFloat) _param_ca_mc_r3_pz, + (ParamFloat) _param_ca_mc_r3_ax, + (ParamFloat) _param_ca_mc_r3_ay, + (ParamFloat) _param_ca_mc_r3_az, + (ParamFloat) _param_ca_mc_r3_ct, + (ParamFloat) _param_ca_mc_r3_km, + + (ParamFloat) _param_ca_mc_r4_px, + (ParamFloat) _param_ca_mc_r4_py, + (ParamFloat) _param_ca_mc_r4_pz, + (ParamFloat) _param_ca_mc_r4_ax, + (ParamFloat) _param_ca_mc_r4_ay, + (ParamFloat) _param_ca_mc_r4_az, + (ParamFloat) _param_ca_mc_r4_ct, + (ParamFloat) _param_ca_mc_r4_km, + + (ParamFloat) _param_ca_mc_r5_px, + (ParamFloat) _param_ca_mc_r5_py, + (ParamFloat) _param_ca_mc_r5_pz, + (ParamFloat) _param_ca_mc_r5_ax, + (ParamFloat) _param_ca_mc_r5_ay, + (ParamFloat) _param_ca_mc_r5_az, + (ParamFloat) _param_ca_mc_r5_ct, + (ParamFloat) _param_ca_mc_r5_km, + + (ParamFloat) _param_ca_mc_r6_px, + (ParamFloat) _param_ca_mc_r6_py, + (ParamFloat) _param_ca_mc_r6_pz, + (ParamFloat) _param_ca_mc_r6_ax, + (ParamFloat) _param_ca_mc_r6_ay, + (ParamFloat) _param_ca_mc_r6_az, + (ParamFloat) _param_ca_mc_r6_ct, + (ParamFloat) _param_ca_mc_r6_km, + + (ParamFloat) _param_ca_mc_r7_px, + (ParamFloat) _param_ca_mc_r7_py, + (ParamFloat) _param_ca_mc_r7_pz, + (ParamFloat) _param_ca_mc_r7_ax, + (ParamFloat) _param_ca_mc_r7_ay, + (ParamFloat) _param_ca_mc_r7_az, + (ParamFloat) _param_ca_mc_r7_ct, + (ParamFloat) _param_ca_mc_r7_km + ) +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c new file mode 100644 index 000000000000..2fea6eac571a --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c @@ -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 + */ + +/** + * 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); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp new file mode 100644 index 000000000000..69b60caf341f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -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 + */ + +#include +#include + +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 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 effectiveness_expected( + expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp new file mode 100644 index 000000000000..c5b2b8bb7574 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -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 + */ + +#include "ActuatorEffectivenessStandardVTOL.hpp" + +ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} + +bool +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + if (!_updated) { + return false; + } + + 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} + }; + matrix = matrix::Matrix(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} + }; + matrix = matrix::Matrix(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} + }; + matrix = matrix::Matrix(standard_vtol); + break; + } + } + + _updated = false; + return true; +} + +void +ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + _updated = true; + +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp new file mode 100644 index 000000000000..135dc976df4d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessStandardVTOL(); + virtual ~ActuatorEffectivenessStandardVTOL() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + + int numActuators() const override { return 7; } +protected: + bool _updated{true}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp new file mode 100644 index 000000000000..c2001386a9a4 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 + */ + +#include "ActuatorEffectivenessTiltrotorVTOL.hpp" + +ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} +bool +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) +{ + if (!_updated) { + return false; + } + + // 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} + }; + matrix = matrix::Matrix(tiltrotor_vtol); + + // Temporarily disable a few controls (WIP) + for (size_t j = 4; j < 8; j++) { + matrix(3, j) = 0.0f; + matrix(4, j) = 0.0f; + matrix(5, j) = 0.0f; + } + + + _updated = false; + return true; +} + +void +ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + + _updated = true; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp new file mode 100644 index 000000000000..3c266b833c55 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTiltrotorVTOL(); + virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + + int numActuators() const override { return 10; } +protected: + bool _updated{true}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 000000000000..50eec6edcf6c --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -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) diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt new file mode 100644 index 000000000000..06a0ff017a3e --- /dev/null +++ b/src/modules/control_allocator/CMakeLists.txt @@ -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 +) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt new file mode 100644 index 000000000000..ecf5f0d2e447 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -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) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp new file mode 100644 index 000000000000..9fac9f55c841 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 + */ + +#include "ControlAllocation.hpp" + +void +ControlAllocation::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) +{ + _effectiveness = effectiveness; + _actuator_trim = actuator_trim; + clipActuatorSetpoint(_actuator_trim); + _control_trim = _effectiveness * _actuator_trim; + _num_actuators = num_actuators; + + // make sure unused actuators are initialized to trim + for (int i = num_actuators; i < NUM_ACTUATORS; ++i) { + _actuator_sp(i) = _actuator_trim(i); + } +} + +void +ControlAllocation::setActuatorSetpoint( + const matrix::Vector &actuator_sp) +{ + // Set actuator setpoint + _actuator_sp = actuator_sp; + + // Clip + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; + +} + +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_max(i) < _actuator_min(i)) { + actuator(i) = _actuator_trim(i); + + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); + + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); + } + } +} + +matrix::Vector +ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) +const +{ + matrix::Vector 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; +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp new file mode 100644 index 000000000000..86a2cfc8ed61 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include +#include + +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 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 &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators); + + /** + * Get the allocated actuator vector + * + * @return Actuator vector + */ + const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + const matrix::Vector &getControlSetpoint() const { return _control_sp; } + + /** + * Get the allocated control vector + * + * @return Control vector + */ + const matrix::Vector &getAllocatedControl() const { return _control_allocated; } + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const { return _effectiveness; } + + /** + * Set the minimum actuator values + * + * @param actuator_min Minimum actuator values + */ + void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } + + /** + * Get the minimum actuator values + * + * @return Minimum actuator values + */ + const matrix::Vector &getActuatorMin() const { return _actuator_min; } + + /** + * Set the maximum actuator values + * + * @param actuator_max Maximum actuator values + */ + void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } + + /** + * Get the maximum actuator values + * + * @return Maximum actuator values + */ + const matrix::Vector &getActuatorMax() const { return _actuator_max; } + + /** + * 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 &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 + */ + void clipActuatorSetpoint(matrix::Vector &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 normalizeActuatorSetpoint(const matrix::Vector &actuator) + const; + + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + +protected: + matrix::Matrix _effectiveness; //< Effectiveness matrix + matrix::Vector _actuator_trim; //< Neutral actuator values + matrix::Vector _actuator_min; //< Minimum actuator values + matrix::Vector _actuator_max; //< Maximum actuator values + matrix::Vector _actuator_sp; //< Actuator setpoint + matrix::Vector _control_sp; //< Control setpoint + matrix::Vector _control_allocated; //< Allocated control + matrix::Vector _control_trim; //< Control at trim actuator values + int _num_actuators{0}; +}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp new file mode 100644 index 000000000000..b613e77c739c --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -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 + */ + +#include "ControlAllocationPseudoInverse.hpp" + +void +ControlAllocationPseudoInverse::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) +{ + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators); + _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 + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp new file mode 100644 index 000000000000..148b5af6cdb8 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -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 + */ + +#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 &effectiveness, + const matrix::Vector &actuator_trim, int num_actuators) override; + +protected: + matrix::Matrix _mix; + + bool _mix_update_needed{false}; + + /** + * Recalculate pseudo inverse if required. + * + */ + void updatePseudoInverse(); +}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp new file mode 100644 index 000000000000..fe04cca748bc --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -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 + */ + +#include +#include + +using namespace matrix; + +TEST(ControlAllocationTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector actuator_sp_expected; + + method.setEffectivenessMatrix(effectiveness, actuator_trim); + 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); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp new file mode 100644 index 000000000000..a497fd8c68ad --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * 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 + * @author Beat Küng + */ + +#include "ControlAllocationSequentialDesaturation.hpp" + + +void +ControlAllocationSequentialDesaturation::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; + + case 2: + mixAirmodeRPY(); + break; + + default: + mixAirmodeDisabled(); + break; + } + + // TODO: thrust model (THR_MDL_FAC) + + // Clip + clipActuatorSetpoint(_actuator_sp); + + // Compute achieved control + _control_allocated = _effectiveness * _actuator_sp; +} + +void ControlAllocationSequentialDesaturation::desaturateActuators( + ActuatorVector &actuator_sp, + const ActuatorVector &desaturation_vector, bool increase_only) +{ + float gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + if (increase_only && gain < 0.f) { + return; + } + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } + + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } +} + +float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, + const ActuatorVector &actuator_sp) +{ + float k_min = 0.f; + float k_max = 0.f; + + for (int 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; +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * 0.15f; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp new file mode 100644 index 000000000000..53c422cd3940 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include "ControlAllocationPseudoInverse.hpp" + +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams +{ +public: + + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} + virtual ~ControlAllocationSequentialDesaturation() = default; + + void allocate() override; + + void updateParameters() override; +private: + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector + */ + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); + + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * + * @return desaturation gain + */ + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + void mixAirmodeRP(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); +}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp new file mode 100644 index 000000000000..1bb0ae312fad --- /dev/null +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * 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 + */ + +#include "ControlAllocator.hpp" + +#include +#include +#include +#include + +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() +{ + delete _control_allocation; + delete _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(); + + if (_control_allocation == nullptr) { + return; + } + + // Minimum actuator values + matrix::Vector 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 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); +} + +void +ControlAllocator::update_allocation_method() +{ + AllocationMethod method = (AllocationMethod)_param_ca_method.get(); + + if (_allocation_method_id != method) { + + // Save current state + matrix::Vector 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"); + 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 { + // Swap allocation methods + delete _control_allocation; + _control_allocation = tmp; + + // Save method id + _allocation_method_id = method; + + // Configure new allocation method + _control_allocation->setActuatorSetpoint(actuator_sp); + } + } +} + +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"); + 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 { + // Swap effectiveness sources + delete _actuator_effectiveness; + _actuator_effectiveness = tmp; + + // Save source id + _effectiveness_source_id = source; + } + } +} + +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(¶m_update); + + if (_control_allocation) { + _control_allocation->updateParameters(); + } + + updateParams(); + parameters_updated(); + } + + if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) { + return; + } + + vehicle_status_s vehicle_status; + + if (_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); + + 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) { + _last_run = now; + + update_effectiveness_matrix_if_needed(); + + // Set control setpoint vector + matrix::Vector 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::update_effectiveness_matrix_if_needed() +{ + matrix::Matrix effectiveness; + + if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) { + const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector 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 + _control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators()); + } +} + +void +ControlAllocator::publish_actuator_setpoint() +{ + matrix::Vector 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 + const matrix::Vector &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 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_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), + unallocated_control(5)).norm_squared() < 1e-6f); + + // Actuator saturation + const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation->getActuatorMin(); + const matrix::Vector &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 actuator_sp = _control_allocation->getActuatorSetpoint(); + matrix::Vector 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 &effectiveness = _control_allocation->getEffectivenessMatrix(); + PX4_INFO("Effectiveness.T ="); + effectiveness.T().print(); + PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators()); + } + + // 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); +} diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp new file mode 100644 index 000000000000..9a1045401200 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * 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 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ControlAllocator : public ModuleBase, 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 update_effectiveness_matrix_if_needed(); + + 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_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */ + uORB::Publication _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_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */ + uORB::Publication _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 _last_run{0}; + hrt_abstime _timestamp_sample{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe, + (ParamInt) _param_ca_method, + (ParamBool) _param_ca_bat_scale_en, + (ParamBool) _param_ca_air_scale_en, + (ParamFloat) _param_ca_act0_min, + (ParamFloat) _param_ca_act1_min, + (ParamFloat) _param_ca_act2_min, + (ParamFloat) _param_ca_act3_min, + (ParamFloat) _param_ca_act4_min, + (ParamFloat) _param_ca_act5_min, + (ParamFloat) _param_ca_act6_min, + (ParamFloat) _param_ca_act7_min, + (ParamFloat) _param_ca_act8_min, + (ParamFloat) _param_ca_act9_min, + (ParamFloat) _param_ca_act10_min, + (ParamFloat) _param_ca_act11_min, + (ParamFloat) _param_ca_act12_min, + (ParamFloat) _param_ca_act13_min, + (ParamFloat) _param_ca_act14_min, + (ParamFloat) _param_ca_act15_min, + (ParamFloat) _param_ca_act0_max, + (ParamFloat) _param_ca_act1_max, + (ParamFloat) _param_ca_act2_max, + (ParamFloat) _param_ca_act3_max, + (ParamFloat) _param_ca_act4_max, + (ParamFloat) _param_ca_act5_max, + (ParamFloat) _param_ca_act6_max, + (ParamFloat) _param_ca_act7_max, + (ParamFloat) _param_ca_act8_max, + (ParamFloat) _param_ca_act9_max, + (ParamFloat) _param_ca_act10_max, + (ParamFloat) _param_ca_act11_max, + (ParamFloat) _param_ca_act12_max, + (ParamFloat) _param_ca_act13_max, + (ParamFloat) _param_ca_act14_max, + (ParamFloat) _param_ca_act15_max + ) + +}; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c new file mode 100644 index 000000000000..c1384d4414b7 --- /dev/null +++ b/src/modules/control_allocator/control_allocator_params.c @@ -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 + */ + + +/** + * 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); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a760c7beab2f..e1693c5a121d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -46,6 +46,8 @@ void LoggedTopics::add_default_topics() { add_topic("actuator_controls_0", 100); add_topic("actuator_controls_1", 100); + add_topic("actuator_controls_4", 100); + add_topic("actuator_controls_5", 100); add_topic("airspeed", 1000); add_topic("airspeed_validated", 1000); add_topic("camera_capture"); @@ -97,6 +99,13 @@ void LoggedTopics::add_default_topics() add_topic("vtol_vehicle_status", 200); add_topic("yaw_estimator_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); add_topic_multi("logger_status"); @@ -217,6 +226,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) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9f7a0b327e08..475d4e76bc40 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -311,6 +311,9 @@ void Standard::update_transition_state() } } + + _v_att_sp->thrust_body[0] = _pusher_throttle; + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); _mc_roll_weight = mc_weight;