From 2f55dff8b98000fd5f86a551482cf14ffc09d1fb Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Wed, 23 Jul 2025 17:26:27 +0200 Subject: [PATCH] feat: spacecraft support (#24734) * rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * feat: spacecraft tooling for commander and VehicleStatus * feat: spacecraft tooling for commander and VehicleStatus * fix: format * fix: format * fix: remove iostream * fix: remove iostream * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: mavlink compliant spacecraft definition * feat: add orbiter to define * boards: Increase TELEM2 rx buffer size for DDS over serial use-case (ARK Jetson) feat: spacecraft tooling for commander and VehicleStatus fix: format fix: remove iostream feat: mavlink compliant spacecraft definition * feat: add orbiter to define * feat: add orbiter to define * fix: change mav_type to new spacecraft orbiter enum value * fix: build issue * feat: update mavlink * feat: update mavlink to latest master with spacecraft * feat: update mavlink * feat: update mavlink to latest * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: removed unnecessary definition * fix: format * feat: cmake variant for spacecraft * feat: proper mav_type and rc init * fix: removed dart from build system * add: thrusters to actuator type * rft: reordering actuator type * rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * fix: format * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: add orbiter to define * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: conflicts * fix: format * fix: remove duplicate entry * rft: remove Kconfig changes * rft: revert main Kconfig * rft: revert main kcoonfig on platforms * rft: remove changes to board PWm (go on another PR) * rft: revert changes to commander (main is correct) * fix: extra char on commander_helper * rft: removed extra spaces * rft: moved effectiveness to spacecraft * fix: spacecraft effectiveness * fix: extra space * feat: preliminary version, still using thrusters * rft: initial pipeline on PX4 side with rotors instead of thrusters * feat: add atmos model * feat: spacecraft with rotor pipeline tested, working * feat: update gz * rft: removed thruster interfaces * fix: format * fix: remove control allocation * fix: thruster normalization * fix: format * fix: nuttx version * fix: clang tidy error * feat: updated gz to add atmos model * fix: update gz * fix: update mavlink * fix: remove friend class from allocation lib * fix: remove actuator_outputs/motors --------- Co-authored-by: Alexander Lerach --- .../init.d-posix/airframes/71001_gz_atmos | 145 ++++++ .../airframes/71002_gz_spacecraft_2d | 168 ++++--- .../init.d-posix/airframes/CMakeLists.txt | 1 + .../init.d/airframes/70001_atmos | 150 +++++++ ROMFS/px4fmu_common/init.d/rc.sc_apps | 30 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 2 +- boards/px4/sitl/spacecraft.px4board | 2 +- .../control_allocator/ControlAllocator.cpp | 4 +- .../control_allocator/ControlAllocator.hpp | 1 + .../ActuatorEffectivenessSpacecraft.cpp | 64 +++ .../ActuatorEffectivenessSpacecraft.hpp | 68 +++ .../CMakeLists.txt | 2 + .../simulation/gz_plugins/CMakeLists.txt | 5 +- .../spacecraft_thruster/CMakeLists.txt | 68 +++ .../SpacecraftThrusterModel.cpp | 389 ++++++++++++++++ .../SpacecraftThrusterModel.hpp | 89 ++++ src/modules/spacecraft/CMakeLists.txt | 8 +- .../AttitudeControl/AttitudeControl.cpp | 94 ++++ .../AttitudeControl/AttitudeControl.hpp | 105 +++++ .../AttitudeControl/AttitudeControlMath.hpp | 70 +++ .../AttitudeControl/CMakeLists.txt | 44 ++ .../ScAttitudeControlMathTest.cpp | 92 ++++ .../AttitudeControl/ScAttitudeControlTest.cpp | 140 ++++++ .../SpacecraftAttitudeControl/CMakeLists.txt | 42 ++ .../SpacecraftAttitudeControl.cpp | 229 ++++++++++ .../SpacecraftAttitudeControl.hpp | 138 ++++++ src/modules/spacecraft/SpacecraftHandler.cpp | 66 ++- src/modules/spacecraft/SpacecraftHandler.hpp | 36 +- .../SpacecraftPositionControl/CMakeLists.txt | 42 ++ .../PositionControl/CMakeLists.txt | 44 ++ .../PositionControl/ControlMath.cpp | 254 +++++++++++ .../PositionControl/ControlMath.hpp | 118 +++++ .../PositionControl/PositionControl.cpp | 220 +++++++++ .../PositionControl/PositionControl.hpp | 206 +++++++++ .../PositionControl/ScControlMathTest.cpp | 258 +++++++++++ .../PositionControl/ScPositionControlTest.cpp | 368 +++++++++++++++ .../SpacecraftPositionControl.cpp | 401 +++++++++++++++++ .../SpacecraftPositionControl.hpp | 177 ++++++++ .../SpacecraftRateControl/CMakeLists.txt | 41 ++ .../SpacecraftRateControl.cpp | 261 +++++++++++ .../SpacecraftRateControl.hpp | 160 +++++++ src/modules/spacecraft/module.yaml | 235 ---------- .../spacecraft/spacecraft_attitude_params.c | 183 ++++++++ .../spacecraft/spacecraft_position_params.c | 293 ++++++++++++ .../spacecraft/spacecraft_rate_params.c | 421 ++++++++++++++++++ 45 files changed, 5573 insertions(+), 361 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/71001_gz_atmos create mode 100644 ROMFS/px4fmu_common/init.d/airframes/70001_atmos create mode 100644 src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp create mode 100644 src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp create mode 100644 src/modules/simulation/gz_plugins/spacecraft_thruster/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.cpp create mode 100644 src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.hpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp create mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp create mode 100644 src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp create mode 100644 src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt create mode 100644 src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp create mode 100644 src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp delete mode 100644 src/modules/spacecraft/module.yaml create mode 100644 src/modules/spacecraft/spacecraft_attitude_params.c create mode 100644 src/modules/spacecraft/spacecraft_position_params.c create mode 100644 src/modules/spacecraft/spacecraft_rate_params.c diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gz_atmos b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gz_atmos new file mode 100644 index 0000000000..c17a92a305 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gz_atmos @@ -0,0 +1,145 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=atmos} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 45 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 0 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_PZ 0.0 +param set-default CA_ROTOR0_CT 1.4 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_PZ 0.0 +param set-default CA_ROTOR1_CT 1.4 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_PZ 0.0 +param set-default CA_ROTOR2_CT 1.4 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_PZ 0.0 +param set-default CA_ROTOR3_CT 1.4 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 + +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_PZ 0.0 +param set-default CA_ROTOR4_CT 1.4 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 + +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_PZ 0.0 +param set-default CA_ROTOR5_CT 1.4 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 + +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_PZ 0.0 +param set-default CA_ROTOR6_CT 1.4 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 + +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_PZ 0.0 +param set-default CA_ROTOR7_CT 1.4 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_FUNC5 105 +param set-default SIM_GZ_EC_FUNC6 106 +param set-default SIM_GZ_EC_FUNC7 107 +param set-default SIM_GZ_EC_FUNC8 108 + +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MIN2 0 +param set-default SIM_GZ_EC_MIN3 0 +param set-default SIM_GZ_EC_MIN4 0 +param set-default SIM_GZ_EC_MIN5 0 +param set-default SIM_GZ_EC_MIN6 0 +param set-default SIM_GZ_EC_MIN7 0 +param set-default SIM_GZ_EC_MIN8 0 + +param set-default SIM_GZ_EC_MAX1 10000 +param set-default SIM_GZ_EC_MAX2 10000 +param set-default SIM_GZ_EC_MAX3 10000 +param set-default SIM_GZ_EC_MAX4 10000 +param set-default SIM_GZ_EC_MAX5 10000 +param set-default SIM_GZ_EC_MAX6 10000 +param set-default SIM_GZ_EC_MAX7 10000 +param set-default SIM_GZ_EC_MAX8 10000 + +# Controller Tunings +param set SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 6f2860a598..68dd6f3aeb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -17,18 +17,16 @@ param set-default SIM_GZ_EN 1 param set-default SENS_EN_MAGSIM 1 param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs -param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? +param set-default FD_ESCS_EN 0 param set-default CA_AIRFRAME 14 param set-default MAV_TYPE 45 -param set-default CA_THRUSTER_CNT 8 +param set-default CA_ROTOR_COUNT 8 param set-default CA_R_REV 0 -# param set-default FW_ARSP_MODE 1 - # Auto to be provided by Custom Airframe -param set-default CA_METHOD 0 # 0 is PseudoInverse, 3 is Metric +param set-default CA_METHOD 0 # Set proper failsafes param set-default COM_ACT_FAIL_ACT 0 @@ -42,96 +40,96 @@ param set-default COM_POSCTL_NAVL 2 param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 -param set-default CA_THRUSTER0_PX -0.12 -param set-default CA_THRUSTER0_PY -0.12 -param set-default CA_THRUSTER0_PZ 0.0 -param set-default CA_THRUSTER0_CT 1.4 -param set-default CA_THRUSTER0_AX 1.0 -param set-default CA_THRUSTER0_AY 0.0 -param set-default CA_THRUSTER0_AZ 0.0 +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_PZ 0.0 +param set-default CA_ROTOR0_CT 1.4 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 -param set-default CA_THRUSTER1_PX 0.12 -param set-default CA_THRUSTER1_PY -0.12 -param set-default CA_THRUSTER1_PZ 0.0 -param set-default CA_THRUSTER1_CT 1.4 -param set-default CA_THRUSTER1_AX -1.0 -param set-default CA_THRUSTER1_AY 0.0 -param set-default CA_THRUSTER1_AZ 0.0 +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_PZ 0.0 +param set-default CA_ROTOR1_CT 1.4 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 -param set-default CA_THRUSTER2_PX -0.12 -param set-default CA_THRUSTER2_PY 0.12 -param set-default CA_THRUSTER2_PZ 0.0 -param set-default CA_THRUSTER2_CT 1.4 -param set-default CA_THRUSTER2_AX 1.0 -param set-default CA_THRUSTER2_AY 0.0 -param set-default CA_THRUSTER2_AZ 0.0 +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_PZ 0.0 +param set-default CA_ROTOR2_CT 1.4 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 -param set-default CA_THRUSTER3_PX 0.12 -param set-default CA_THRUSTER3_PY 0.12 -param set-default CA_THRUSTER3_PZ 0.0 -param set-default CA_THRUSTER3_CT 1.4 -param set-default CA_THRUSTER3_AX -1.0 -param set-default CA_THRUSTER3_AY 0.0 -param set-default CA_THRUSTER3_AZ 0.0 +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_PZ 0.0 +param set-default CA_ROTOR3_CT 1.4 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 -param set-default CA_THRUSTER4_PX 0.12 -param set-default CA_THRUSTER4_PY -0.12 -param set-default CA_THRUSTER4_PZ 0.0 -param set-default CA_THRUSTER4_CT 1.4 -param set-default CA_THRUSTER4_AX 0.0 -param set-default CA_THRUSTER4_AY 1.0 -param set-default CA_THRUSTER4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_PZ 0.0 +param set-default CA_ROTOR4_CT 1.4 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 -param set-default CA_THRUSTER5_PX 0.12 -param set-default CA_THRUSTER5_PY 0.12 -param set-default CA_THRUSTER5_PZ 0.0 -param set-default CA_THRUSTER5_CT 1.4 -param set-default CA_THRUSTER5_AX 0.0 -param set-default CA_THRUSTER5_AY -1.0 -param set-default CA_THRUSTER5_AZ 0.0 +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_PZ 0.0 +param set-default CA_ROTOR5_CT 1.4 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 -param set-default CA_THRUSTER6_PX -0.12 -param set-default CA_THRUSTER6_PY -0.12 -param set-default CA_THRUSTER6_PZ 0.0 -param set-default CA_THRUSTER6_CT 1.4 -param set-default CA_THRUSTER6_AX 0.0 -param set-default CA_THRUSTER6_AY 1.0 -param set-default CA_THRUSTER6_AZ 0.0 +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_PZ 0.0 +param set-default CA_ROTOR6_CT 1.4 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 -param set-default CA_THRUSTER7_PX -0.12 -param set-default CA_THRUSTER7_PY 0.12 -param set-default CA_THRUSTER7_PZ 0.0 -param set-default CA_THRUSTER7_CT 1.4 -param set-default CA_THRUSTER7_AX 0.0 -param set-default CA_THRUSTER7_AY -1.0 -param set-default CA_THRUSTER7_AZ 0.0 +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_PZ 0.0 +param set-default CA_ROTOR7_CT 1.4 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 -param set-default SIM_GZ_TH_FUNC1 101 -param set-default SIM_GZ_TH_FUNC2 102 -param set-default SIM_GZ_TH_FUNC3 103 -param set-default SIM_GZ_TH_FUNC4 104 -param set-default SIM_GZ_TH_FUNC5 105 -param set-default SIM_GZ_TH_FUNC6 106 -param set-default SIM_GZ_TH_FUNC7 107 -param set-default SIM_GZ_TH_FUNC8 108 +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_FUNC5 105 +param set-default SIM_GZ_EC_FUNC6 106 +param set-default SIM_GZ_EC_FUNC7 107 +param set-default SIM_GZ_EC_FUNC8 108 -param set-default SIM_GZ_TH_MIN1 0 -param set-default SIM_GZ_TH_MIN2 0 -param set-default SIM_GZ_TH_MIN3 0 -param set-default SIM_GZ_TH_MIN4 0 -param set-default SIM_GZ_TH_MIN5 0 -param set-default SIM_GZ_TH_MIN6 0 -param set-default SIM_GZ_TH_MIN7 0 -param set-default SIM_GZ_TH_MIN8 0 +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MIN2 0 +param set-default SIM_GZ_EC_MIN3 0 +param set-default SIM_GZ_EC_MIN4 0 +param set-default SIM_GZ_EC_MIN5 0 +param set-default SIM_GZ_EC_MIN6 0 +param set-default SIM_GZ_EC_MIN7 0 +param set-default SIM_GZ_EC_MIN8 0 -param set-default SIM_GZ_TH_MAX1 10000 -param set-default SIM_GZ_TH_MAX2 10000 -param set-default SIM_GZ_TH_MAX3 10000 -param set-default SIM_GZ_TH_MAX4 10000 -param set-default SIM_GZ_TH_MAX5 10000 -param set-default SIM_GZ_TH_MAX6 10000 -param set-default SIM_GZ_TH_MAX7 10000 -param set-default SIM_GZ_TH_MAX8 10000 +param set-default SIM_GZ_EC_MAX1 10000 +param set-default SIM_GZ_EC_MAX2 10000 +param set-default SIM_GZ_EC_MAX3 10000 +param set-default SIM_GZ_EC_MAX4 10000 +param set-default SIM_GZ_EC_MAX5 10000 +param set-default SIM_GZ_EC_MAX6 10000 +param set-default SIM_GZ_EC_MAX7 10000 +param set-default SIM_GZ_EC_MAX8 10000 # Controller Tunings param set SC_YAWRATE_P 3.335 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index b621344331..eb2fc90c49 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -115,6 +115,7 @@ px4_add_romfs_files( 50000_gz_rover_differential + 71001_gz_atmos 71002_gz_spacecraft_2d # [22000, 22999] Reserve for custom models diff --git a/ROMFS/px4fmu_common/init.d/airframes/70001_atmos b/ROMFS/px4fmu_common/init.d/airframes/70001_atmos new file mode 100644 index 0000000000..7ecf3b666b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70001_atmos @@ -0,0 +1,150 @@ +#!/bin/sh +# +# @name KTH Space Robot +# +# @type Space Robot +# @class 2D Space Robot +# +# @maintainer DISCOWER +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 45 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 0 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# Set Mocap Vision frame +param set EKF2_EV_CTRL 15 +param set EKF2_HGT_REF 3 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.4 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.4 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.4 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.4 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.4 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.4 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.4 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.4 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + + +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index 2df68601ac..17ea03d6e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -5,32 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -# Start Spacecraft App +control_allocator start + spacecraft start - -# Estimator Group Selection -# ekf2 start & - -# Start MicroDDS Client -# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 -# uxrce_dds_client start -t udp -p 8888 - -# -# Start Control Allocator -# -# sc_control_allocator start - -# -# Start Spacecraft Rate Controller. -# -# sc_rate_control start - -# -# Start Spacecraft Attitude Controller. -# -# sc_att_control start - -# -# Start Spacecraft Position Controller. -# -# sc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 43e97b3785..0f10a34ba0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -69,7 +69,7 @@ then fi # -# Spapcecraft setup. +# Spacecraft setup. # if [ $VEHICLE_TYPE = spacecraft ] then diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board index 44ac04d7dd..3cc3f185b6 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -16,5 +16,5 @@ CONFIG_MODULES_ROVER_ACKERMANN=n CONFIG_MODULES_ROVER_DIFFERENTIAL=n CONFIG_MODULES_ROVER_MECANUM=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y -CONFIG_MODULES_CONTROL_ALLOCATOR=n +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_SPACECRAFT=y diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index a7e43a64e3..8b322ef94c 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -266,11 +266,11 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::SPACECRAFT_2D: - // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + tmp = new ActuatorEffectivenessSpacecraft(this); break; case EffectivenessSource::SPACECRAFT_3D: - // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + tmp = new ActuatorEffectivenessSpacecraft(this); break; default: diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index ce92c5036b..14c30026d1 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp new file mode 100644 index 0000000000..ad13d213cd --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "ActuatorEffectivenessSpacecraft.hpp" + +using namespace matrix; + +ActuatorEffectivenessSpacecraft::ActuatorEffectivenessSpacecraft(ModuleParams *parent) + : ModuleParams(parent), + _sc_thrusters(this) +{ +} + +bool +ActuatorEffectivenessSpacecraft::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Thrusters + const bool thrusters_added_successfully = _sc_thrusters.addActuators(configuration); + + return thrusters_added_successfully; +} + +void ActuatorEffectivenessSpacecraft::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + // TODO(@E-Krantz): Here is where we can add the nonlinearity of multiple thrusters open + // and how to adjust thrust in that time +} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp new file mode 100644 index 0000000000..e2fb530f73 --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" + +class ActuatorEffectivenessSpacecraft : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessSpacecraft(ModuleParams *parent); + virtual ~ActuatorEffectivenessSpacecraft() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const + override + { + // TODO(@Jaeyoung-Lim): to be updated + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): to be updated + normalize[0] = true; + } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + const char *name() const override { return "Spacecraft"; } + +protected: + ActuatorEffectivenessRotors _sc_thrusters; +}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt index 349893a3ea..e50e77f550 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -19,6 +19,8 @@ px4_add_library(VehicleActuatorEffectiveness ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessSpacecraft.cpp + ActuatorEffectivenessSpacecraft.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index cd4b7e7c70..265bbd9599 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -56,11 +56,12 @@ if (gz-transport_FOUND) add_subdirectory(template_plugin) add_subdirectory(gstreamer) add_subdirectory(moving_platform_controller) + add_subdirectory(spacecraft_thruster) # Add an alias target for each plugin if (TARGET GstCameraSystem) - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GstCameraSystem) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem) else() - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin) endif() endif() diff --git a/src/modules/simulation/gz_plugins/spacecraft_thruster/CMakeLists.txt b/src/modules/simulation/gz_plugins/spacecraft_thruster/CMakeLists.txt new file mode 100644 index 0000000000..5f196526dd --- /dev/null +++ b/src/modules/simulation/gz_plugins/spacecraft_thruster/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(SpacecraftThrusterModelPlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + SpacecraftThrusterModel.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR} + PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR} + PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR} + PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.cpp b/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.cpp new file mode 100644 index 0000000000..846d794de4 --- /dev/null +++ b/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.cpp @@ -0,0 +1,389 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * Copyright (C) 2024 Open Source Robotics Foundation + * Copyright (C) 2024 Benjamin Perseghetti, Rudis Laboratories + * Copyright (C) 2024 Pedro Roque, DCS, KTH, Sweden + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "SpacecraftThrusterModel.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include "gz/sim/components/Actuators.hh" +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::SpacecraftThrusterModelPrivate +{ + /// \brief Callback for actuator commands. +public: void OnActuatorMsg(const msgs::Actuators &_msg); + + /// \brief Apply link forces and moments based on propeller state. +public: void UpdateForcesAndMoments(EntityComponentManager &_ecm); + + /// \brief Link Entity +public: Entity linkEntity; + + /// \brief Link name +public: std::string linkName; + + /// \brief Model interface +public: Model model{kNullEntity}; + + /// \brief sub topic for actuator commands. +public: std::string subTopic; + + /// \brief Topic namespace. +public: std::string topic; + + /// \brief Simulation time tracker +public: double simTime = 0.01; + + /// \brief Index of motor in Actuators msg on multirotor_base. +public: int actuatorNumber = 0; + + /// \brief Duty cycle frequency +public: double dutyCycleFrequency = 10.0; + + /// \brief Cycle start time +public: double cycleStartTime = 0.0; + + /// \brief Sampling time with the cycle period. +public: double samplingTime = 0.01; + + /// \brief Actuator maximum thrust +public: double maxThrust = 0.0; + + /// \brief Maximum duty cycle value. Use the same value as in PX4 + /// maximum PWM__MAXi value. For the thruster, it is assumed + /// that the minimum value is 0 (solenoid closed). +public: double maxDutyCycle = 10000.0; + + /// \brief Received Actuators message. This is nullopt if no message has been + /// received. +public: std::optional recvdActuatorsMsg; + + /// \brief Mutex to protect recvdActuatorsMsg. +public: std::mutex recvdActuatorsMsgMutex; + + /// \brief Gazebo communication node. +public: transport::Node node; +}; + +////////////////////////////////////////////////// +SpacecraftThrusterModel::SpacecraftThrusterModel() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void SpacecraftThrusterModel::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) { + gzerr << "SpacecraftThrusterModel plugin should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } + + auto sdfClone = _sdf->Clone(); + + this->dataPtr->topic.clear(); + + if (sdfClone->HasElement("topic")) { + this->dataPtr->topic = + sdfClone->Get("topic"); + + } else { + gzwarn << "No topic set using entity name.\n"; + this->dataPtr->topic = this->dataPtr->model.Name(_ecm); + } + + if (sdfClone->HasElement("link_name")) { + this->dataPtr->linkName = sdfClone->Get("link_name"); + } + + if (this->dataPtr->linkName.empty()) { + gzerr << "SpacecraftThrusterModel found an empty link_name parameter. " + << "Failed to initialize."; + return; + } + + if (sdfClone->HasElement("actuator_number")) { + this->dataPtr->actuatorNumber = + sdfClone->GetElement("actuator_number")->Get(); + + } else { + gzerr << "Please specify a actuator_number.\n"; + } + + if (sdfClone->HasElement("max_thrust")) { + this->dataPtr->maxThrust = + sdfClone->GetElement("max_thrust")->Get(); + + } else { + gzerr << "Please specify actuator " + << this->dataPtr->actuatorNumber << " max_thrust.\n"; + } + + if (sdfClone->HasElement("duty_cycle_frequency")) { + this->dataPtr->dutyCycleFrequency = + sdfClone->GetElement("duty_cycle_frequency")->Get(); + + } else { + gzerr << "Please specify actuator " + << this->dataPtr->actuatorNumber << " duty_cycle_frequency.\n"; + } + + if (sdfClone->HasElement("max_duty_cycle")) { + this->dataPtr->maxDutyCycle = + sdfClone->GetElement("max_duty_cycle")->Get(); + + if (this->dataPtr->maxDutyCycle <= 0.0) { + gzerr << "Please specify a positive max_duty_cycle.\n"; + return; + } + + } else { + gzerr << "Please specify actuator " + << this->dataPtr->actuatorNumber << " max_duty_cycle.\n"; + } + + std::string topic; + + if (sdfClone->HasElement("sub_topic")) { + this->dataPtr->subTopic = + sdfClone->Get("sub_topic"); + topic = transport::TopicUtils::AsValidTopic( + this->dataPtr->topic + "/" + this->dataPtr->subTopic); + + } else { + topic = transport::TopicUtils::AsValidTopic( + this->dataPtr->topic); + } + + // Subscribe to actuator command message + if (topic.empty()) { + gzerr << "Failed to create topic for [" << this->dataPtr->topic + << "]" << std::endl; + return; + + } else { + gzdbg << "Listening to topic: " << topic << std::endl; + } + + this->dataPtr->node.Subscribe(topic, + &SpacecraftThrusterModelPrivate::OnActuatorMsg, this->dataPtr.get()); + + // Look for components + // If the link hasn't been identified yet, look for it + if (this->dataPtr->linkEntity == kNullEntity) { + this->dataPtr->linkEntity = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + } + + if (this->dataPtr->linkEntity == kNullEntity) { + gzerr << "Failed to find link entity. " + << "Failed to initialize." << std::endl; + return; + } + + // skip UpdateForcesAndMoments if needed components are missing + bool providedAllComponents = true; + + if (!_ecm.Component(this->dataPtr->linkEntity)) { + _ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose()); + } + + if (!providedAllComponents) { + gzdbg << "Created necessary components." << std::endl; + } + +} + +////////////////////////////////////////////////// +void SpacecraftThrusterModelPrivate::OnActuatorMsg( + const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->recvdActuatorsMsgMutex); + this->recvdActuatorsMsg = _msg; + + if (this->actuatorNumber == 0) { + gzdbg << "Received actuator message!" << std::endl; + } +} + +////////////////////////////////////////////////// +void SpacecraftThrusterModel::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("SpacecraftThrusterModel::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Nothing left to do if paused. + if (_info.paused) { + return; + } + + this->dataPtr->simTime = std::chrono::duration(_info.simTime).count(); + this->dataPtr->UpdateForcesAndMoments(_ecm); +} + +////////////////////////////////////////////////// +void SpacecraftThrusterModelPrivate::UpdateForcesAndMoments( + EntityComponentManager &_ecm) +{ + GZ_PROFILE("SpacecraftThrusterModelPrivate::UpdateForcesAndMoments"); + std::optional msg; + auto actuatorMsgComp = + _ecm.Component(this->model.Entity()); + + // Actuators messages can come in from transport or via a component. If a + // component is available, it takes precedence. + if (actuatorMsgComp) { + msg = actuatorMsgComp->Data(); + + } else { + std::lock_guard lock(this->recvdActuatorsMsgMutex); + + if (this->recvdActuatorsMsg.has_value()) { + msg = *this->recvdActuatorsMsg; + } + } + + if (msg.has_value()) { + if (this->actuatorNumber > msg->velocity_size() - 1) { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator array which is of size " + << msg->velocity_size() << std::endl; + return; + } + + } else { + return; + } + + // Normalize input + double normalizedInput = + msg->velocity(this->actuatorNumber) / this->maxDutyCycle; + + // METHOD: + // + // targetDutyCycle starts as a normalized value between 0 and 1, so we + // need to convert it to the corresponding time in the duty cycle + // period + // |________| |________ + // | ^ | | | + // __| | |____| |__ + // a b c d + // a: cycle start time + // b: sampling time + // c: target duty cycle + // d: cycle period + double targetDutyCycle = normalizedInput * (1.0 / this->dutyCycleFrequency); + + if (this->actuatorNumber == 0) + gzdbg << this->actuatorNumber + << ": target duty cycle: " << targetDutyCycle << std::endl; + + // Calculate cycle start time + if (this->samplingTime >= 1.0 / this->dutyCycleFrequency) { + if (this->actuatorNumber == 0) + gzdbg << this->actuatorNumber + << ": Cycle completed. Resetting cycle start time." + << std::endl; + + this->cycleStartTime = this->simTime; + } + + // Calculate sampling time instant within the cycle + this->samplingTime = this->simTime - this->cycleStartTime; + + if (this->actuatorNumber == 0) + gzdbg << this->actuatorNumber + << ": PWM Period: " << 1.0 / this->dutyCycleFrequency + << " Cycle Start time: " << this->cycleStartTime + << " Sampling time: " << this->samplingTime << std::endl; + + // Apply force if the sampling time is less than the target ON duty cycle + double force = this->samplingTime <= targetDutyCycle ? this->maxThrust : 0.0; + + if (targetDutyCycle < 1e-9) { force = 0.0; } + + if (this->actuatorNumber == 0) + gzdbg << this->actuatorNumber + << ": Force: " << force + << " Sampling time: " << this->samplingTime + << " Tgt duty cycle: " << targetDutyCycle << std::endl; + + // Apply force to the link + Link link(this->linkEntity); + const auto worldPose = link.WorldPose(_ecm); + link.AddWorldForce(_ecm, + worldPose->Rot().RotateVector(math::Vector3d(0, 0, force))); + + if (this->actuatorNumber == 0) + gzdbg << this->actuatorNumber + << ": Input Value: " << msg->velocity(this->actuatorNumber) + << " Calc. Force: " << force << std::endl; +} + +GZ_ADD_PLUGIN(SpacecraftThrusterModel, + System, + SpacecraftThrusterModel::ISystemConfigure, + SpacecraftThrusterModel::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(SpacecraftThrusterModel, + "gz::sim::systems::SpacecraftThrusterModel") diff --git a/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.hpp b/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.hpp new file mode 100644 index 0000000000..94127dd755 --- /dev/null +++ b/src/modules/simulation/gz_plugins/spacecraft_thruster/SpacecraftThrusterModel.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_SPACECRAFTTHRUSTERMODEL_HH_ +#define GZ_SIM_SYSTEMS_SPACECRAFTTHRUSTERMODEL_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ +// Forward declaration +class SpacecraftThrusterModelPrivate; + +/// \brief This system applies a thrust force to models with RCS-like +/// thrusters. See tutorials/spacecraft_thrusters.md for a tutorial usage. +/// Below follow the minimum necessary parameters needed by the plugin: +/// \param link_name Name of the link that the thruster is attached to. +/// \param actuator_number Index of the element to be used from the +/// actuators message for a joint. +/// \param duty_cycle_frequency Frequency of the duty cycle signal in Hz. +/// \param max_thrust Maximum thrust force in Newtons, applied on the +/// «on» phase of the duty cycle. +/// \param topic Name of the topic where the commanded normalized +/// thrust is published. Unit is <0, 1>, corresponding to the +/// percentage of the duty cycle that the thruster is on. +/// Default uses the models name. +/// \param sub_topic [optional] Name of the sub_topic to listen to actuator +/// message on. +/// +/// This plugin replicates the PWM thruster behavior in: +/// Nakka, Yashwanth Kumar, et al. "A six degree-of-freedom spacecraft +/// dynamics simulator for formation control research." 2018 AAS/AIAA +/// Astrodynamics Specialist Conference. 2018. -> 'Thruster Firing Time' +/// Phodapol, S. (2023). Predictive Controllers for Load Transportation in +/// Microgravity Environments (Dissertation). -> '5.3.4 PWM Controller Node' +/// Retrieved from https://urn.kb.se/resolve?urn=urn:nbn:se:kth:diva-344440 + +class SpacecraftThrusterModel + : public System, + public ISystemConfigure, + public ISystemPreUpdate +{ + /// \brief Constructor +public: SpacecraftThrusterModel(); + + /// \brief Destructor +public: ~SpacecraftThrusterModel() override = default; + + // Documentation inherited +public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited +public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer +private: std::unique_ptr dataPtr; +}; +} +} +} +} + +#endif diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt index 88a18e9c01..7d39e24ece 100644 --- a/src/modules/spacecraft/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -32,6 +32,9 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(SpacecraftRateControl) +add_subdirectory(SpacecraftAttitudeControl) +add_subdirectory(SpacecraftPositionControl) px4_add_module( MODULE modules__spacecraft @@ -43,9 +46,10 @@ px4_add_module( SRCS SpacecraftHandler.cpp SpacecraftHandler.hpp - MODULE_CONFIG - module.yaml DEPENDS mathlib px4_work_queue + SpacecraftRateControl + SpacecraftAttitudeControl + SpacecraftPositionControl ) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 0000000000..7c335485ea --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 AttitudeControl.cpp + */ + +#include + +#include + +using namespace matrix; + +void ScAttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain) +{ + _proportional_gain = proportional_gain; +} + +matrix::Vector3f ScAttitudeControl::update(const Quatf &q) const +{ + Quatf qd = _attitude_setpoint_q; + + // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch + const Vector3f e_z = q.dcm_z(); + const Vector3f e_z_d = qd.dcm_z(); + Quatf qd_red(e_z, e_z_d); + + if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) { + // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, + // full attitude control anyways generates no yaw input and directly takes the combination of + // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. + qd_red = qd; + + } else { + // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + qd_red *= q; + } + + // mix full and reduced desired attitude + Quatf q_mix = qd_red.inversed() * qd; + q_mix.canonicalize(); + + // catch numerical problems with the domain of acosf and asinf + q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); + q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); + qd = qd_red * Quatf(q_mix(0), 0, 0, q_mix(3)); + + // quaternion attitude control law, qe is rotation from q to qd + const Quatf qe = q.inversed() * qd; + + // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + // also taking care of the antipodal unit quaternion ambiguity + const Vector3f eq = 2.f * qe.canonical().imag(); + + // calculate angular rates setpoint + Vector3f rate_setpoint = eq.emult(_proportional_gain); + + // limit rates + for (int i = 0; i < 3; i++) { + rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); + } + + return rate_setpoint; +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 0000000000..3d702055b8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 AttitudeControl.hpp + * + * A quaternion based attitude controller. + * + * @author Matthias Grob + * + * Publication documenting the implemented Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich + * + * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + */ + +#pragma once + +#include +#include + +class ScAttitudeControl +{ +public: + ScAttitudeControl() = default; + ~ScAttitudeControl() = default; + + /** + * Set proportional attitude control gain + * @param proportional_gain 3D vector containing gains for roll, pitch, yaw + */ + void setProportionalGain(const matrix::Vector3f &proportional_gain); + + /** + * Set hard limit for output rate setpoints + * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw + */ + void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + + /** + * Set a new attitude setpoint replacing the one tracked before + * @param qd desired vehicle attitude setpoint + */ + void setAttitudeSetpoint(const matrix::Quatf &qd) + { + _attitude_setpoint_q = qd; + _attitude_setpoint_q.normalize(); + } + + /** + * Adjust last known attitude setpoint by a delta rotation + * Optional use to avoid glitches when attitude estimate reference e.g. heading changes. + * @param q_delta delta rotation to apply + */ + void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) + { + _attitude_setpoint_q = q_delta * _attitude_setpoint_q; + _attitude_setpoint_q.normalize(); + } + + /** + * Run one control loop cycle calculation + * @param q estimation of the current vehicle attitude unit quaternion + * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller + */ + matrix::Vector3f update(const matrix::Quatf &q) const; + +private: + matrix::Vector3f _proportional_gain; + matrix::Vector3f _rate_limit; + + matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control +}; diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp new file mode 100644 index 0000000000..fe0f760ed4 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 AttitudeControlMath.hpp + */ + +#pragma once + +#include + +namespace AttitudeControlMath +{ +/** + * Rotate a tilt quaternion (without yaw rotation) such that when rotated by a yaw setpoint + * the resulting tilt is the same as if it was rotated by the current yaw of the vehicle + * @param q_sp_tilt pure tilt quaternion (yaw = 0) that needs to be corrected + * @param q_att current attitude of the vehicle + * @param q_sp_yaw pure yaw quaternion of the desired yaw setpoint + */ +void inline correctTiltSetpointForYawError(matrix::Quatf &q_sp_tilt, const matrix::Quatf &q_att, + const matrix::Quatf &q_sp_yaw) +{ + const matrix::Vector3f z_unit(0.f, 0.f, 1.f); + + // Extract yaw from the current attitude + const matrix::Vector3f att_z = q_att.dcm_z(); + const matrix::Quatf q_tilt(z_unit, att_z); + const matrix::Quatf q_yaw = q_tilt.inversed() * q_att; // This is not euler yaw + + // Find the quaternion that creates a tilt aligned with the body frame + // when rotated by the yaw setpoint + // To do so, solve q_yaw * q_tilt_ne = q_sp_yaw * q_sp_rp_compensated + const matrix::Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_tilt; + + // Extract the corrected tilt + const matrix::Vector3f att_sp_z = q_sp_rp_compensated.dcm_z(); + q_sp_tilt = matrix::Quatf(z_unit, att_sp_z); +} +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..825f96d2f9 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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(AttitudeControlLibrary + AttitudeControl.cpp + AttitudeControl.hpp + AttitudeControlMath.hpp +) +target_compile_options(AttitudeControlLibrary PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(AttitudeControlLibrary PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# TODO: Add unit tests +# px4_add_unit_gtest(SRC ScAttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl) +# px4_add_unit_gtest(SRC ScAttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp new file mode 100644 index 0000000000..b247fce074 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "AttitudeControlMath.hpp" + +using namespace matrix; +using namespace AttitudeControlMath; + +static const Vector3f z_unit(0.f, 0.f, 1.f); + +TEST(AttitudeControlMath, tiltCorrectionNoError) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + + // AND: a desired yaw setpoint + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: the current yaw error is zero (regardless of the tilt) + const Quatf q = q_sp_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt setpoint is unchanged + EXPECT_TRUE(isEqual(q_tilt_sp_ne_before, q_tilt_sp_ne)); +} + +TEST(AttitudeControlMath, tiltCorrectionYaw180) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -M_PI_F / 2.f); + + // WHEN: there is a yaw error of 180 degrees + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, M_PI_F / 2.f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt is reversed (the corrected tilt angle is the same but the axis of rotation is opposite) + EXPECT_FLOAT_EQ(AxisAnglef(q_tilt_sp_ne_before).angle(), AxisAnglef(q_tilt_sp_ne).angle()); + EXPECT_TRUE(isEqual(AxisAnglef(q_tilt_sp_ne_before).axis(), -AxisAnglef(q_tilt_sp_ne).axis())); +} + +TEST(AttitudeControlMath, tiltCorrection) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(0.5, -0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: there is a some yaw error + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, 3.1f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt vector obtained by rotating the corrected tilt by the yaw setpoint is the same as + // the one obtained by rotating the initial tilt by the current yaw of the vehicle + EXPECT_TRUE(isEqual((q_sp_yaw * q_tilt_sp_ne).dcm_z(), (q_yaw * q_tilt_sp_ne_before).dcm_z())); +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp new file mode 100644 index 0000000000..c7c20c5a62 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace matrix; + +TEST(ScAttitudeControlTest, AllZeroCase) +{ + ScAttitudeControl attitude_control; + Vector3f rate_setpoint = attitude_control.update(Quatf()); + EXPECT_EQ(rate_setpoint, Vector3f()); +} + +class ScAttitudeControlConvergenceTest : public ::testing::Test +{ +public: + ScAttitudeControlConvergenceTest() + { + _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f)); + _attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); + } + + void checkConvergence() + { + int i; // need function scope to check how many steps + Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); + + _attitude_control.setAttitudeSetpoint(_quat_goal); + + for (i = 100; i > 0; i--) { + // run attitude control to get rate setpoints + const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state); + // rotate the simulated state quaternion according to the rate setpoint + _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); + _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem + + // expect the error and hence also the output to get smaller with each iteration + if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { + break; + } + + rate_setpoint = rate_setpoint_new; + } + + EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical()); + // it shouldn't have taken longer than an iteration timeout to converge + EXPECT_GT(i, 0); + } + + ScAttitudeControl _attitude_control; + Quatf _quat_state; + Quatf _quat_goal; +}; + +TEST_F(ScAttitudeControlConvergenceTest, AttitudeControlConvergence) +{ + const int inputs = 8; + + const Quatf QArray[inputs] = { + Quatf(), + Quatf(0, 1, 0, 0), + Quatf(0, 0, 1, 0), + Quatf(0, 0, 0, 1), + Quatf(0.698f, 0.024f, -0.681f, -0.220f), + Quatf(-0.820f, -0.313f, 0.225f, -0.423f), + Quatf(0.599f, -0.172f, 0.755f, -0.204f), + Quatf(0.216f, -0.662f, 0.290f, -0.656f) + }; + + for (int i = 0; i < inputs; i++) { + for (int j = 0; j < inputs; j++) { + printf("--- Input combination: %d %d\n", i, j); + _quat_state = QArray[i]; + _quat_goal = QArray[j]; + _quat_state.normalize(); + _quat_goal.normalize(); + checkConvergence(); + } + } +} + +TEST(ScAttitudeControlTest, YawWeightScaling) +{ + // GIVEN: default tuning and pure yaw turn command + ScAttitudeControl attitude_control; + const float yaw_gain = 2.8f; + const float yaw_sp = .1f; + Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); + attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude); + + // WHEN: we run one iteration of the controller + Vector3f rate_setpoint = attitude_control.update(Quatf()); + + // THEN: no actuation in roll, pitch + EXPECT_EQ(Vector2f(rate_setpoint), Vector2f()); + // THEN: actuation error * gain in yaw + EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); + + // GIVEN: additional corner case of zero yaw weight + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); + // WHEN: we run one iteration of the controller + rate_setpoint = attitude_control.update(Quatf()); + // THEN: no actuation (also no NAN) + EXPECT_EQ(rate_setpoint, Vector3f()); +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..0985fe3f19 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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_subdirectory(AttitudeControl) + +px4_add_library(SpacecraftAttitudeControl +SpacecraftAttitudeControl.cpp +) + +target_link_libraries(SpacecraftAttitudeControl PUBLIC mathlib) +target_link_libraries(SpacecraftAttitudeControl PUBLIC AttitudeControlLibrary) +target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp new file mode 100644 index 0000000000..5636eaac75 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 sc_att_control_main.cpp + * Spacecraft attitude controller. + * Based off the multicopter attitude controller module. + * + * @author Pedro Roque, + * + */ + +#include "SpacecraftAttitudeControl.hpp" + +#include +#include +#include + +#include "AttitudeControl/AttitudeControlMath.hpp" + +using namespace matrix; + +SpacecraftAttitudeControl::SpacecraftAttitudeControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void +SpacecraftAttitudeControl::updateParams() +{ + ModuleParams::updateParams(); + // Store some of the parameters in a more convenient way & precompute often-used values + _attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get())); + + // angular rate limits + using math::radians; + _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), + radians(_param_sc_yawrate_max.get()))); + _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); +} + +void +SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(q).psi(); + + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_sc_man_y_scale.get()); + + // Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true + if (_manual_control_setpoint.throttle < -.9f) { + reset_yaw_sp = true; + } + + // Make sure not absolute heading error builds up + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else { + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(roll*roll + pitch*pitch) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + _man_roll_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + _man_pitch_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + + Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), + -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f)); + + // Align the desired tilt with the yaw setpoint + Quatf q_sp = q_sp_yaw * q_sp_rp; + + q_sp.copyTo(attitude_setpoint.q_d); + + // Transform to euler angles for logging only + // const Eulerf euler_sp(q_sp); + // attitude_setpoint.roll_body = euler_sp(0); + // attitude_setpoint.pitch_body = euler_sp(1); + // attitude_setpoint.yaw_body = euler_sp(2); + // attitude_setpoint.q_d[0] = q_sp; + + attitude_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // update attitude controller setpoint immediately + _attitude_control.setAttitudeSetpoint(q_sp); + _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); + _last_attitude_setpoint = attitude_setpoint.timestamp; +} + +void +SpacecraftAttitudeControl::updateAttitudeControl() +{ + // run controller on attitude updates + vehicle_attitude_s v_att; + + if (_vehicle_attitude_sub.update(&v_att)) { + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp_sample; + + const Quatf q{v_att.q}; + + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + + if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) + && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d)); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; + } + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + + if (v_att.timestamp > _last_attitude_setpoint) { + // adapt existing attitude setpoint unless it was generated after the current attitude estimate + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + } + + _quat_reset_counter = v_att.quat_reset_counter; + } + + /* check for updates in other topics */ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + _heading_good_for_control = vehicle_local_position.heading_good_for_control; + } + } + + bool attitude_setpoint_generated = false; + + if (_vehicle_control_mode.flag_control_attitude_enabled) { + + // Generate the attitude setpoint from stick inputs if we are in Stabilized mode + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_altitude_enabled && + !_vehicle_control_mode.flag_control_velocity_enabled && + !_vehicle_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(q, dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + + } else { + _man_roll_input_filter.reset(0.f); + _man_pitch_input_filter.reset(0.f); + } + + Vector3f rates_sp = _attitude_control.update(q); + + // publish rate setpoint + vehicle_rates_setpoint_s rates_setpoint{}; + rates_setpoint.roll = rates_sp(0); + rates_setpoint.pitch = rates_sp(1); + rates_setpoint.yaw = rates_sp(2); + _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); + rates_setpoint.timestamp = hrt_absolute_time(); + _vehicle_rates_setpoint_pub.publish(rates_setpoint); + } + + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control; + } +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp new file mode 100644 index 0000000000..1e87b94295 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 + +class SpacecraftAttitudeControl : public ModuleParams +{ +public: + SpacecraftAttitudeControl(ModuleParams *parent); + ~SpacecraftAttitudeControl() = default; + + void updateAttitudeControl(); + +protected: + + /** + * initialize some vectors/matrices from parameters + */ + void updateParams(); + +private: + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); + + ScAttitudeControl _attitude_control; /**< class for attitude control calculations */ + + // Attitude setpoint and current attitude sub + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + + // Manual control setpoint sub + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + // Vehicle control mode sub and status + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + // Vehicle local position sub + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + // Publish rate setpoint for rate controller and att_control status + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; /**< attitude setpoint publication */ + + manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ + vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */ + + matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + + AlphaFilter _man_roll_input_filter; + AlphaFilter _man_pitch_input_filter; + + hrt_abstime _last_run{0}; + hrt_abstime _last_attitude_setpoint{0}; + + bool _reset_yaw_sp{true}; + bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published + bool _vehicle_type_spacecraft{true}; + + uint8_t _quat_reset_counter{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_man_tilt_tau, + + (ParamFloat) _param_sc_roll_p, + (ParamFloat) _param_sc_pitch_p, + (ParamFloat) _param_sc_yaw_p, + (ParamFloat) _param_sc_yaw_weight, + + (ParamFloat) _param_sc_rollrate_max, + (ParamFloat) _param_sc_pitchrate_max, + (ParamFloat) _param_sc_yawrate_max, + (ParamFloat) _param_sc_man_tilt_max, + + /* Stabilized mode params */ + (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ + ) +}; diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp index 92cdf86655..6e6ecf7a6a 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -34,16 +34,75 @@ /** * @file SpacecraftHandler.cpp * - * Control allocator. + * Spacecraft control handler. * - * @author Julien Lecoeur + * @author Pedro Roque */ #include "SpacecraftHandler.hpp" + +using namespace time_literals; + +SpacecraftHandler::SpacecraftHandler() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool SpacecraftHandler::init() +{ + ScheduleOnInterval(4_ms); // 250 Hz + return true; +} + +void SpacecraftHandler::updateParams() +{ + ModuleParams::updateParams(); +} + +void SpacecraftHandler::Run() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + _spacecraft_position_control.updatePositionControl(); + _spacecraft_attitude_control.updateAttitudeControl(); + _spacecraft_rate_control.updateRateControl(); + +} + int SpacecraftHandler::task_spawn(int argc, char *argv[]) { - return 0; + SpacecraftHandler *instance = new SpacecraftHandler(); + + 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 SpacecraftHandler::print_status() @@ -75,6 +134,7 @@ int SpacecraftHandler::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("spacecraft", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("status"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index 8fc954dc71..732347cd97 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -62,13 +62,18 @@ #include #include +// Local includes +#include "SpacecraftRateControl/SpacecraftRateControl.hpp" +#include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp" +#include "SpacecraftPositionControl/SpacecraftPositionControl.hpp" + class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: SpacecraftHandler(); - virtual ~SpacecraftHandler(); + ~SpacecraftHandler() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -82,6 +87,33 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; -private: /**< loop duration performance counter */ + bool init(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + void Run() override; + + // uORB subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + + // Class instances + SpacecraftRateControl _spacecraft_rate_control{this}; + SpacecraftAttitudeControl _spacecraft_attitude_control{this}; + SpacecraftPositionControl _spacecraft_position_control{this}; + + // Variables + hrt_abstime _timestamp{0}; + float _dt{0.f}; }; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt new file mode 100644 index 0000000000..6a3bc3f018 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +add_subdirectory(PositionControl) + +px4_add_library(SpacecraftPositionControl + SpacecraftPositionControl.cpp +) + +target_link_libraries(SpacecraftPositionControl PUBLIC mathlib) +target_link_libraries(SpacecraftPositionControl PUBLIC PositionControlLibrary) +target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt new file mode 100644 index 0000000000..58d10293f1 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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(PositionControlLibrary + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(PositionControlLibrary PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# TODO: add unit tests +# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl) +# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl) diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp new file mode 100644 index 0000000000..b13acd5122 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp @@ -0,0 +1,254 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.cpp + */ + +#include "ControlMath.hpp" +#include +#include +#include + +using namespace matrix; + +namespace ControlMath +{ +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + +void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + // zero vector, no direction, set safe level value + if (body_z.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + } + + body_z.normalize(); + + // vector of desired yaw direction in XY plane, rotated by PI/2 + const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f}; + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.f; + } + + body_x.normalize(); + + // desired body_y axis + const Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + const Quatf q_sp{R_sp}; + q_sp.copyTo(att_sp.q_d); +} + +Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) +{ + if (Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.dot(v0) - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} + +bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, + const Vector3f &line_a, const Vector3f &line_b, Vector3f &res) +{ + // project center of sphere on line normalized AB + Vector3f ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + + ab_norm.normalize(); + Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + if (sphere_r > cd_len) { + // we have triangle CDX with known CD and CX = R, find DX + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + + if ((sphere_c - line_b) * ab_norm > 0.f) { + // target waypoint is already behind us + res = line_b; + + } else { + // target is in front of us + res = d + ab_norm * dx_len; // vector A->B on line + } + + return true; + + } else { + + // have no roots, return D + res = d; // go directly to line + + // previous waypoint is still in front of us + if ((sphere_c - line_a) * ab_norm < 0.f) { + res = line_a; + } + + // target waypoint is already behind us + if ((sphere_c - line_b) * ab_norm > 0.f) { + res = line_b; + } + + return false; + } +} + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do +} + +void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector3f(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector3f(vector, Vector3f()); +} +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp new file mode 100644 index 0000000000..c2d9b8b7f8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.hpp + * + * Simple functions for vector manipulation that do not fit into matrix lib. + * These functions are specific for controls. + */ + +#pragma once + +#include +#include + +namespace ControlMath +{ +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp desired 3D thrust vector + * @param yaw_sp the desired yaw + * @param att_sp attitude setpoint to fill + */ +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is too big + * @param world_unit fixed vector to measure the angle against + * @param max_angle maximum tilt angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + +/** + * Converts a body z vector and yaw set-point to a desired attitude. + * @param body_z a world frame 3D vector in direction of the desired body z axis + * @param yaw_sp the desired yaw setpoint + * @param att_sp attitude setpoint to fill + */ +void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max); + +/** + * This method was used for smoothing the corners along two lines. + * + * @param sphere_c + * @param sphere_r + * @param line_a + * @param line_b + * @param res + * return boolean + * + * Note: this method is not used anywhere and first requires review before usage. + */ +bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, + const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommitted value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector3f(matrix::Vector3f &vector); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp new file mode 100644 index 0000000000..c1ed93896a --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.cpp + */ + +#include "PositionControl.hpp" +#include "ControlMath.hpp" +#include +#include +#include +#include +#include + +using namespace matrix; + +const trajectory_setpoint6dof_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}}; + +void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void ScPositionControl::setPositionGains(const Vector3f &P, const Vector3f &I) +{ + _gain_pos_p = P; + _gain_pos_i = I; +} + +void ScPositionControl::setPositionIntegralLimits(const float lim) +{ + _pos_int_lim = lim; +} + +void ScPositionControl::setVelocityIntegralLimits(const float lim) +{ + _vel_int_lim = lim; +} + +void ScPositionControl::setVelocityLimits(const float vel_limit) +{ + _lim_vel = vel_limit; +} + +void ScPositionControl::setThrustLimit(const float max) +{ + _lim_thr_max = max; +} + +void ScPositionControl::setState(const PositionControlStates &states) +{ + _pos = states.position; + _vel = states.velocity; + _vel_dot = states.acceleration; + _att_q = states.quaternion; +} + +void ScPositionControl::setInputSetpoint(const trajectory_setpoint6dof_s &setpoint) +{ + _pos_sp = Vector3f(setpoint.position); + _vel_sp = Vector3f(setpoint.velocity); + _acc_sp = Vector3f(setpoint.acceleration); + _quat_sp = Quatf(setpoint.quaternion); +} + +bool ScPositionControl::update(const float dt) +{ + bool valid = _inputValid(); + + if (valid) { + _positionControl(dt); + _velocityControl(dt); + } + + // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong + return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite(); +} + +void ScPositionControl::_positionControl(const float dt) +{ + // Constrain vertical velocity integral + _pos_int(0) = math::constrain(_vel_int(0), -_pos_int_lim, _pos_int_lim); + _pos_int(1) = math::constrain(_vel_int(1), -_pos_int_lim, _pos_int_lim); + _pos_int(2) = math::constrain(_vel_int(2), -_pos_int_lim, _pos_int_lim); + + // P-position controller + ControlMath::setZeroIfNanVector3f(_pos_sp); + Vector3f pos_error = _pos_sp - _pos; + Vector3f vel_sp_position = pos_error.emult(_gain_pos_p) + _pos_int; + + // Update integral part of position control + _vel_int += pos_error.emult(_gain_pos_i) * dt; + + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence + ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); + // make sure there are no NAN elements for further reference while constraining + ControlMath::setZeroIfNanVector3f(vel_sp_position); + + // Constrain velocity setpoints + _vel_sp(0) = math::constrain(_vel_sp(0), -_lim_vel, _lim_vel); + _vel_sp(1) = math::constrain(_vel_sp(1), -_lim_vel, _lim_vel); + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel, _lim_vel); + +} + +void ScPositionControl::_velocityControl(const float dt) +{ + // Constrain vertical velocity integral + // _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G); + // Constrain vertical velocity integral + _vel_int(0) = math::constrain(_vel_int(0), -_vel_int_lim, _vel_int_lim); + _vel_int(1) = math::constrain(_vel_int(1), -_vel_int_lim, _vel_int_lim); + _vel_int(2) = math::constrain(_vel_int(2), -_vel_int_lim, _vel_int_lim); + + + // PID velocity control + Vector3f vel_error = _vel_sp - _vel; + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); + + // Accelaration to Thrust + _thr_sp = _acc_sp; + _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); + _thr_sp(1) = math::constrain(_thr_sp(1), -_lim_thr_max, _lim_thr_max); + _thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, _lim_thr_max); + + // Make sure integral doesn't get NAN + ControlMath::setZeroIfNanVector3f(vel_error); + + // Update integral part of velocity control + _vel_int += vel_error.emult(_gain_vel_i) * dt; +} + +bool ScPositionControl::_inputValid() +{ + bool valid = true; + + // Every axis x, y, z needs to have some setpoint + for (int i = 0; i <= 2; i++) { + valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i))); + } + + // x and y input setpoints always have to come in pairs + valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))); + valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))); + valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + // For each controlled state the estimate has to be valid + for (int i = 0; i <= 2; i++) { + if (PX4_ISFINITE(_pos_sp(i))) { + valid = valid && PX4_ISFINITE(_pos(i)); + } + + if (PX4_ISFINITE(_vel_sp(i))) { + valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i)); + } + } + + return valid; +} + +void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, + vehicle_attitude_s &v_att) const +{ + // Set thrust setpoint + const Dcmf R_to_body(Quatf(v_att.q).inversed()); + matrix::Vector3f b_thr_sp = R_to_body * _thr_sp; + attitude_setpoint.thrust_body[0] = b_thr_sp(0); + attitude_setpoint.thrust_body[1] = b_thr_sp(1); + attitude_setpoint.thrust_body[2] = b_thr_sp(2); + + // Bypass attitude control by giving same attitude setpoint to att control + if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) { + attitude_setpoint.q_d[0] = _quat_sp(0); + attitude_setpoint.q_d[1] = _quat_sp(1); + attitude_setpoint.q_d[2] = _quat_sp(2); + attitude_setpoint.q_d[3] = _quat_sp(3); + + } else { + attitude_setpoint.q_d[0] = v_att.q[0]; + attitude_setpoint.q_d[1] = v_att.q[1]; + attitude_setpoint.q_d[2] = v_att.q[2]; + attitude_setpoint.q_d[3] = v_att.q[3]; + } +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp new file mode 100644 index 0000000000..a4dc628bb0 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f acceleration; + matrix::Quatf quaternion; // bypassed to attitude controller +}; + +/** + * Core Position-Control for spacecrafts. + * This class contains P-controller for position and + * PID-controller for velocity. + * + * Inputs: + * vehicle position/velocity/attitude + * desired set-point position/velocity/thrust/attitude + * Output + * thrust vector and quaternion for attitude control + * + * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. + */ +class ScPositionControl +{ +public: + + ScPositionControl() = default; + ~ScPositionControl() = default; + + /** + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains for x,y,z axis + */ + void setPositionGains(const matrix::Vector3f &P, const matrix::Vector3f &I); + + /** + * @brief Set the Position Integral Limits object + * + * @param lim float limit to be set (on all axis) + */ + void setPositionIntegralLimits(const float lim); + + /** + * Set the velocity control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the maximum velocity to execute with feed forward and position control + * @param vel_limit velocity limit + */ + void setVelocityLimits(const float vel_limit); + + /** + * @brief Set the Velocity Integral Limits object + * + * @param lim float limit to be set (on all axis) + */ + void setVelocityIntegralLimits(const float lim); + + /** + * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller + * @param min minimum thrust e.g. 0.1 or 0 + * @param max maximum thrust e.g. 0.9 or 1 + */ + void setThrustLimit(const float max); + + /** + * Pass the current vehicle state to the controller + * @param PositionControlStates structure + */ + void setState(const PositionControlStates &states); + + /** + * Pass the desired setpoints + * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. + * @param setpoint setpoints including feed-forwards to execute in update() + */ + void setInputSetpoint(const trajectory_setpoint6dof_s &setpoint); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt time in seconds since last iteration + * @return true if update succeeded and output setpoint is executable, false if not + */ + bool update(const float dt); + + /** + * Set the integral term in xy to 0. + * @see _vel_int + */ + void resetIntegral() + { + _pos_int.setZero(); + _vel_int.setZero(); + } + + /** + * Get the controllers output attitude setpoint + * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. + * It needs to be executed by the attitude controller to achieve velocity and position tracking. + * @param attitude_setpoint reference to struct to fill up + */ + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const; + + /** + * All setpoints are set to NAN (uncontrolled). Timestampt zero. + */ + static const trajectory_setpoint6dof_s empty_trajectory_setpoint; + +private: + // The range limits of the hover thrust configuration/estimate + static constexpr float HOVER_THRUST_MIN = 0.05f; + static constexpr float HOVER_THRUST_MAX = 0.9f; + + bool _inputValid(); + + void _positionControl(const float dt); ///< Position PI control + void _velocityControl(const float dt); ///< Velocity PID control + + // Gains + matrix::Vector3f _gain_pos_p; ///< Position control proportional gain + matrix::Vector3f _gain_pos_i; ///< Position control integral gain + matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain + matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain + matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain + + // Limits + float _lim_vel{}; ///< Horizontal velocity limit with feed forward and position control + float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 + float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 + float _pos_int_lim{}; ///< Anti-windup for position control + float _vel_int_lim{}; ///< Anti-windup for velocity control + + // States + matrix::Vector3f _pos; /**< current position */ + matrix::Vector3f _pos_int; /**< integral term of the position controller */ + matrix::Vector3f _vel; /**< current velocity */ + matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ + matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + matrix::Quatf _att_q; /**< current attitude */ + float _yaw{}; /**< current heading */ + + // Setpoints + matrix::Vector3f _pos_sp; /**< desired position */ + matrix::Vector3f _vel_sp; /**< desired velocity */ + matrix::Vector3f _acc_sp; /**< desired acceleration */ + matrix::Vector3f _thr_sp; /**< desired thrust */ + matrix::Quatf _quat_sp; /**< desired attitude */ +}; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp new file mode 100644 index 0000000000..e9b6fba12f --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace matrix; +using namespace ControlMath; + +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0.f, .1f, 1.f).normalized(); + body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1.f, 0.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0.f, 1.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1.f, 1.f, .1f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} + +TEST(ControlMathTest, ThrottleAttitudeMapping) +{ + /* expected: zero roll, zero pitch, zero yaw, full thr mag + * reason: thrust pointing full upward */ + Vector3f thr{0.f, 0.f, -1.f}; + float yaw = 0.f; + vehicle_attitude_setpoint_s att{}; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but with 90 yaw + * reason: only yaw changed */ + yaw = M_PI_2_F; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but roll 180 + * reason: thrust points straight down and order Euler + * order is: 1. roll, 2. pitch, 3. yaw */ + thr = Vector3f(0.f, 0.f, 1.f); + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); +} + +TEST(ControlMathTest, ConstrainXYPriorities) +{ + const float max = 5.f; + // v0 already at max + Vector2f v0(max, 0.f); + Vector2f v1(v0(1), -v0(0)); + + Vector2f v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), max); + EXPECT_FLOAT_EQ(v_r(1), 0.f); + + // norm of v1 exceeds max but v0 is zero + v0.zero(); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(1), -max); + EXPECT_FLOAT_EQ(v_r(0), 0.f); + + v0 = Vector2f(.5f, .5f); + v1 = Vector2f(.5f, -.5f); + v_r = constrainXY(v0, v1, max); + const float diff = Vector2f(v_r - (v0 + v1)).length(); + EXPECT_FLOAT_EQ(diff, 0.f); + + // v0 and v1 exceed max and are perpendicular + v0 = Vector2f(4.f, 0.f); + v1 = Vector2f(0.f, -4.f); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), v0(0)); + EXPECT_GT(v_r(0), 0.f); + const float remaining = sqrtf(max * max - (v0(0) * v0(0))); + EXPECT_FLOAT_EQ(v_r(1), -remaining); +} + +TEST(ControlMathTest, CrossSphereLine) +{ + /* Testing 9 positions (+) around waypoints (o): + * + * Far + + + + * + * Near + + + + * On trajectory --+----o---------+---------o----+-- + * prev curr + * + * Expected targets (1, 2, 3): + * Far + + + + * + * + * On trajectory -------1---------2---------3------- + * + * + * Near + + + + * On trajectory -------o---1---------2-----3------- + * + * + * On trajectory --+----o----1----+--------2/3---+-- */ + const Vector3f prev = Vector3f(0.f, 0.f, 0.f); + const Vector3f curr = Vector3f(0.f, 0.f, 2.f); + Vector3f res; + bool retval = false; + + // on line, near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f)); + + // on line, near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // on line, near, after target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f)); + + // near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f)); + + // near, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // far, before previous waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f()); + + // far, before target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f)); + + // far, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); +} + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(std::isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp new file mode 100644 index 0000000000..a9ce422ed5 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace matrix; + +TEST(PositionControlTest, EmptySetpoint) +{ + ScPositionControl position_control; + + vehicle_attitude_setpoint_s attitude{}; + position_control.getAttitudeSetpoint(attitude); + EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); + EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(attitude.reset_integral, false); + EXPECT_EQ(attitude.fw_control_yaw_wheel, false); +} + +class PositionControlBasicTest : public ::testing::Test +{ +public: + PositionControlBasicTest() + { + _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); + _position_control.setVelocityLimits(1.f, 1.f, 1.f); + _position_control.setThrustLimits(0.1f, MAXIMUM_THRUST); + _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); + _position_control.setTiltLimit(1.f); + _position_control.setHoverThrust(.5f); + } + + bool runController() + { + _position_control.setInputSetpoint(_input_setpoint); + const bool ret = _position_control.update(.1f); + _position_control.getAttitudeSetpoint(_attitude); + return ret; + } + + ScPositionControl _position_control; + trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_local_position_setpoint_s _output_setpoint{}; + vehicle_attitude_setpoint_s _attitude{}; + + static constexpr float MAXIMUM_THRUST = 0.9f; + static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f; +}; + +class PositionControlBasicDirectionTest : public PositionControlBasicTest +{ +public: + void checkDirection() + { + Vector3f thrust(_output_setpoint.thrust); + EXPECT_GT(thrust(0), 0.f); + EXPECT_GT(thrust(1), 0.f); + EXPECT_LT(thrust(2), 0.f); + + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + EXPECT_LT(body_z(0), 0.f); + EXPECT_LT(body_z(1), 0.f); + EXPECT_GT(body_z(2), 0.f); + } +}; + +TEST_F(PositionControlBasicDirectionTest, PositionDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, TiltLimit) +{ + Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, 1.f); + + _position_control.setTiltLimit(0.5f); + EXPECT_TRUE(runController()); + body_z = Quatf(_attitude.q_d).dcm_z(); + angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original +} + +TEST_F(PositionControlBasicTest, VelocityLimit) +{ + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); + EXPECT_LE(velocity_xy.norm(), 1.f); + EXPECT_LE(abs(_output_setpoint.vz), 1.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) +{ + // Given a setpoint that drives the controller into vertical and horizontal saturation + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + // When you run it for one iteration + runController(); + Vector3f thrust(_output_setpoint.thrust); + + // Then the thrust vector length is limited by the maximum + EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST); + + // Then the horizontal thrust is limited by its margin + EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(2), + -sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN)); + thrust.print(); + + // Then the collective thrust is limited by the maximum + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); + + // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust + EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore + // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); +} + +TEST_F(PositionControlBasicTest, FailsafeInput) +{ + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + _input_setpoint.velocity[2] = .1f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_LT(_output_setpoint.thrust[2], -.1f); + EXPECT_GT(_output_setpoint.thrust[2], -.5f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); +} + +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust +} + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.velocity[0] = .1f; + _input_setpoint.velocity[1] = .2f; + _input_setpoint.position[2] = .3f; // altitude + + EXPECT_TRUE(runController()); + EXPECT_TRUE(isnan(_output_setpoint.x)); + EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, SetpointValiditySimple) +{ + EXPECT_FALSE(runController()); + _input_setpoint.position[0] = .1f; + EXPECT_FALSE(runController()); + _input_setpoint.position[1] = .2f; + EXPECT_FALSE(runController()); + _input_setpoint.acceleration[2] = .3f; + EXPECT_TRUE(runController()); +} + +TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) +{ + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], + &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], + &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] + }; + + for (int combination = 0; combination < 512; combination++) { + _input_setpoint = PositionControl::empty_trajectory_setpoint; + + for (int j = 0; j < 9; j++) { + if (combination & (1 << j)) { + // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations + *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); + } + } + + // Expect at least one setpoint per axis + const bool has_x_setpoint = ((combination & 7) != 0); + const bool has_y_setpoint = (((combination >> 3) & 7) != 0); + const bool has_z_setpoint = (((combination >> 6) & 7) != 0); + // Expect xy setpoints to come in pairs + const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); + const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; + + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.position[0] << ", " + << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl + << "velocity " << _input_setpoint.velocity[0] << ", " + << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; + } +} + +TEST_F(PositionControlBasicTest, InvalidState) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + PositionControlStates states{}; + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); +} + +TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) +{ + // GIVEN: the controller was ran with an invalid setpoint containing some valid values + _input_setpoint.position[0] = .1f; + _input_setpoint.position[1] = .2f; + // all z-axis setpoints stay NAN + EXPECT_FALSE(runController()); + + // WHEN: we run the controller with a valid setpoint + _input_setpoint = PositionControl::empty_trajectory_setpoint; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + + // THEN: the integral did not wind up and produce unexpected deviation + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp new file mode 100644 index 0000000000..eccf2bb945 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp @@ -0,0 +1,401 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +#include "SpacecraftPositionControl.hpp" + +#include +#include +#include "PositionControl/ControlMath.hpp" + +using namespace matrix; + +SpacecraftPositionControl::SpacecraftPositionControl(ModuleParams *parent) : ModuleParams(parent), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)) +{ + updateParams(); +} + +void SpacecraftPositionControl::updateParams() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_vel_all.get() >= 0.f) { + float all_vel = _param_mpc_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_cruise.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_max.commit_no_notification(all_vel); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + // Set PI and PID gains, as well as anti-windup limits + _control.setPositionGains( + Vector3f(_param_mpc_pos_p.get(), _param_mpc_pos_p.get(), _param_mpc_pos_p.get()), + Vector3f(_param_mpc_pos_i.get(), _param_mpc_pos_i.get(), _param_mpc_pos_i.get())); + _control.setPositionIntegralLimits(_param_mpc_pos_i_lim.get()); + _control.setVelocityGains( + Vector3f(_param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get()), + Vector3f(_param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get()), + Vector3f(_param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get())); + _control.setVelocityIntegralLimits(_param_mpc_vel_i_lim.get()); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_vel_cruise.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_cruise.set(_param_mpc_vel_max.get()); + _param_mpc_vel_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); + /* EVENT + * @description SPC_VEL_CRUISE is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_vel_max.get()); + _param_mpc_vel_manual.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t"); + /* EVENT + * @description SPC_VEL_MANUAL is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + yaw_rate = math::radians(_param_mpc_man_y_max.get()); + } +} + +PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &vehicle_attitude) +{ + PositionControlStates states; + + const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y); + + // only set position states if valid and finite + if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) { + states.position.xy() = position_xy; + + } else { + states.position(0) = states.position(1) = NAN; + } + + if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) { + states.position(2) = vehicle_local_position.z; + + } else { + states.position(2) = NAN; + } + + const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); + + if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { + states.velocity.xy() = velocity_xy; + + } else { + states.velocity(0) = states.velocity(1) = NAN; + } + + if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { + states.velocity(2) = vehicle_local_position.vz; + + } else { + states.velocity(2) = NAN; + } + + if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) + && PX4_ISFINITE(vehicle_attitude.q[3])) { + states.quaternion = Quatf(vehicle_attitude.q); + + } else { + states.quaternion = Quatf(); + } + + return states; +} + +void SpacecraftPositionControl::updatePositionControl() +{ + vehicle_local_position_s vehicle_local_position; + vehicle_attitude_s v_att; + + if (_local_pos_sub.update(&vehicle_local_position)) { + const float dt = + math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = vehicle_local_position.timestamp_sample; + + if (_vehicle_control_mode_sub.updated()) { + const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled; + + if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (!previous_position_control_enabled && _vehicle_control_mode.flag_control_position_enabled) { + _time_position_control_enabled = _vehicle_control_mode.timestamp; + + } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_control_position_enabled) { + // clear existing setpoint when controller is no longer active + _setpoint = ScPositionControl::empty_trajectory_setpoint; + } + } + } + + // TODO: check if setpoint is different than the previous one and reset integral then + // _control.resetIntegral(); + _trajectory_setpoint_sub.update(&_setpoint); + _vehicle_attitude_sub.update(&v_att); + + // adjust existing (or older) setpoint with any EKF reset deltas + if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.velocity[0] += vehicle_local_position.delta_vxy[0]; + _setpoint.velocity[1] += vehicle_local_position.delta_vxy[1]; + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _setpoint.velocity[2] += vehicle_local_position.delta_vz; + } + + if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) { + _setpoint.position[0] += vehicle_local_position.delta_xy[0]; + _setpoint.position[1] += vehicle_local_position.delta_xy[1]; + } + + if (vehicle_local_position.z_reset_counter != _z_reset_counter) { + _setpoint.position[2] += vehicle_local_position.delta_z; + } + + if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { + // Set proper attitude setpoint with quaternion + // _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); + } + } + + // save latest reset counters + _vxy_reset_counter = vehicle_local_position.vxy_reset_counter; + _vz_reset_counter = vehicle_local_position.vz_reset_counter; + _xy_reset_counter = vehicle_local_position.xy_reset_counter; + _z_reset_counter = vehicle_local_position.z_reset_counter; + _heading_reset_counter = vehicle_local_position.heading_reset_counter; + + PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; + + poll_manual_setpoint(dt, vehicle_local_position, v_att); + + if (_vehicle_control_mode.flag_control_position_enabled) { + // set failsafe setpoint if there hasn't been a new + // trajectory setpoint since position control started + if ((_setpoint.timestamp < _time_position_control_enabled) + && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { + PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); + _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); + } + } + + if (_vehicle_control_mode.flag_control_position_enabled + && (_setpoint.timestamp >= _time_position_control_enabled)) { + + _control.setThrustLimit(_param_mpc_thr_max.get()); + + _control.setVelocityLimits(_param_mpc_vel_max.get()); + + _control.setInputSetpoint(_setpoint); + + _control.setState(states); + + // Run position control + if (!_control.update(dt)) { + _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); + _control.setVelocityLimits(_param_mpc_vel_max.get()); + _control.update(dt); + } + + // Publish attitude setpoint output + vehicle_attitude_setpoint_s attitude_setpoint{}; + _control.getAttitudeSetpoint(attitude_setpoint, v_att); + // PX4_INFO("States: %f %f %f / %f %f %f", (double)states.position(0), (double)states.position(1), + // (double)states.position(2), (double)states.velocity(0), (double)states.velocity(1), + // (double)states.velocity(2)); + // PX4_INFO("Setpoint: %f %f %f / %f %f %f", (double)_setpoint.position[0], (double)_setpoint.position[1], + // (double)_setpoint.position[2], (double)_setpoint.velocity[0], (double)_setpoint.velocity[1], + // (double)_setpoint.velocity[2]); + // PX4_INFO("Control input: %f %f %f / %f %f %f %f", (double)attitude_setpoint.thrust_body[0], (double)attitude_setpoint.thrust_body[1], + // (double)attitude_setpoint.thrust_body[2], (double)attitude_setpoint.q_d[0], (double)attitude_setpoint.q_d[1], + // (double)attitude_setpoint.q_d[2], (double)attitude_setpoint.q_d[3]); + attitude_setpoint.timestamp = hrt_absolute_time(); + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // publish setpoint + publishLocalPositionSetpoint(attitude_setpoint); + } + } +} + +void SpacecraftPositionControl::publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp) +{ + // complete the setpoint data structure + vehicle_local_position_setpoint_s local_position_setpoint{}; + local_position_setpoint.timestamp = hrt_absolute_time(); + + local_position_setpoint.x = _setpoint.position[0]; + local_position_setpoint.y = _setpoint.position[1]; + local_position_setpoint.z = _setpoint.position[2]; + local_position_setpoint.vx = _setpoint.velocity[0]; + local_position_setpoint.vy = _setpoint.velocity[1]; + local_position_setpoint.vz = _setpoint.velocity[2]; + local_position_setpoint.acceleration[0] = _setpoint.acceleration[0]; + local_position_setpoint.acceleration[1] = _setpoint.acceleration[1]; + local_position_setpoint.acceleration[2] = _setpoint.acceleration[2]; + local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; + local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; + local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; + _local_pos_sp_pub.publish(local_position_setpoint); +} + +void SpacecraftPositionControl::poll_manual_setpoint(const float dt, + const vehicle_local_position_s &vehicle_local_position, + const vehicle_attitude_s &_vehicle_att) +{ + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + if (!_vehicle_control_mode.flag_control_offboard_enabled) { + if (_vehicle_control_mode.flag_control_attitude_enabled && + _vehicle_control_mode.flag_control_position_enabled) { + // We are in Stabilized mode + // Generate position setpoints + if (!stabilized_pos_sp_initialized) { + // Initialize position setpoint + target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, + vehicle_local_position.z); + + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + stabilized_pos_sp_initialized = true; + } + + // Update velocity setpoint + Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); + target_pos_sp = target_pos_sp + target_vel_sp * dt; + + // Update _setpoint + _setpoint.position[0] = target_pos_sp(0); + _setpoint.position[1] = target_pos_sp(1); + _setpoint.position[2] = target_pos_sp(2); + + _setpoint.velocity[0] = target_vel_sp(0); + _setpoint.velocity[1] = target_vel_sp(1); + _setpoint.velocity[2] = target_vel_sp(2); + + // Generate attitude setpoints + float yaw_sp_move_rate = 0.0; + + if (_manual_control_setpoint.throttle > -0.9f) { + yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; + } + + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); + const float roll_body = 0.0; + const float pitch_body = 0.0; + + Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); + q_sp.copyTo(_setpoint.quaternion); + + _setpoint.timestamp = hrt_absolute_time(); + + } else { + // We are in Manual mode + stabilized_pos_sp_initialized = false; + } + + } else { + stabilized_pos_sp_initialized = false; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } + } +} + +trajectory_setpoint6dof_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, + const PositionControlStates &states, bool warn) +{ + // rate limit the warnings + warn = warn && (now - _last_warn) > 2_s; + + if (warn) { + PX4_WARN("invalid setpoints"); + _last_warn = now; + } + + trajectory_setpoint6dof_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint; + failsafe_setpoint.timestamp = now; + + failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + return failsafe_setpoint; +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp new file mode 100644 index 0000000000..292251a0fc --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +#include "PositionControl/PositionControl.hpp" + +#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 + +using namespace time_literals; + +class SpacecraftPositionControl : public ModuleParams +{ +public: + SpacecraftPositionControl(ModuleParams *parent); + ~SpacecraftPositionControl() = default; + + void updatePositionControl(); + +protected: + /** + * Update our local parameter cache. + */ + void updateParams(); + +private: + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_position_control_enabled{0}; + hrt_abstime _manual_setpoint_last_called{0}; + + trajectory_setpoint6dof_s _setpoint{ScPositionControl::empty_trajectory_setpoint}; + vehicle_control_mode_s _vehicle_control_mode{}; + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_pos_p, + (ParamFloat) _param_mpc_pos_i, + (ParamFloat) _param_mpc_pos_i_lim, + (ParamFloat) _param_mpc_vel_p_acc, + (ParamFloat) _param_mpc_vel_i_acc, + (ParamFloat) _param_mpc_vel_i_lim, + (ParamFloat) _param_mpc_vel_d_acc, + (ParamFloat) _param_mpc_vel_all, + (ParamFloat) _param_mpc_vel_max, + (ParamFloat) _param_mpc_vel_cruise, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc, + (ParamFloat) _param_mpc_acc_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_thr_max + ); + + matrix::Vector3f target_pos_sp; + float yaw_rate; + bool stabilized_pos_sp_initialized{false}; + + ScPositionControl _control; /**< class for core PID position control */ + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + // Manual setpoints on yaw and reset + bool _reset_yaw_sp{true}; + float _manual_yaw_sp{0.f}; + float _throttle_control{0.f}; + float _yaw_control{0.f}; + + /** + * Check for validity of positon/velocity states. + */ + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att); + + /** + * Check for manual setpoints. + */ + void poll_manual_setpoint(const float dt, const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + + /** + * @brief publishes target setpoint. + * + */ + void publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp); + + /** + * Generate setpoint to bridge no executable setpoint being available. + * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. + * This should only happen briefly when transitioning and never during mode operation or by design. + */ + trajectory_setpoint6dof_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, + bool warn); +}; diff --git a/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt new file mode 100644 index 0000000000..b4f33ce25a --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2025 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(SpacecraftRateControl + SpacecraftRateControl.cpp +) + +target_link_libraries(SpacecraftRateControl PUBLIC RateControl) +target_link_libraries(SpacecraftRateControl PUBLIC mathlib) +target_link_libraries(SpacecraftRateControl PUBLIC circuit_breaker) +target_include_directories(SpacecraftRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp new file mode 100644 index 0000000000..a9f07e76d7 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * 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 "SpacecraftRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacecraftRateControl::SpacecraftRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _controller_status_pub.advertise(); + updateParams(); +} + +void SpacecraftRateControl::updateParams() +{ + ModuleParams::updateParams(); + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); + _manual_force_max = _param_sc_manual_f_max.get(); + _manual_torque_max = _param_sc_manual_t_max.get(); +} + +void SpacecraftRateControl::updateRateControl() +{ + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_attitude_enabled) { + // Here we can be in: Manual Mode or Acro Mode + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + if (_vehicle_control_mode.flag_control_rates_enabled) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{manual_control_setpoint.roll, + -manual_control_setpoint.pitch, + manual_control_setpoint.yaw}; + + _rates_setpoint = man_rate_sp * 5; + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + + } else if (!_vehicle_control_mode.flag_control_rates_enabled) { + // Manual/direct control + // Yaw stick commands rotational moment, Roll/Pitch stick commands translational forces + // All other axis are set as zero (We only have four channels on the manual control inputs) + _thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(2) = 0.0; + + _torque_setpoint(0) = _torque_setpoint(1) = 0.0; + _torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + _torque_setpoint.copyTo(vehicle_torque_setpoint.xyz); + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + // Get rates from other controllers (e.g. position or attitude controller) + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation 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; + } + } + } + } + + const Vector3f torque_sp = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, false); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_sp(0)) ? torque_sp(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_sp(1)) ? torque_sp(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_sp(2)) ? torque_sp(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_sc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } +} + +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp new file mode 100644 index 0000000000..cfb7a620cd --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * 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 +#include + +using namespace time_literals; + +class SpacecraftRateControl : public ModuleParams +{ +public: + SpacecraftRateControl(ModuleParams *parent); + ~SpacecraftRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + void updateParams() override; + +private: + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + vehicle_angular_velocity_s angular_velocity{}; + + bool _landed{true}; + bool _maybe_landed{true}; + + hrt_abstime _last_run{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + // keep setpoint values between updates + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + matrix::Vector3f _rates_setpoint{}; + float _manual_torque_max{1.0}; + float _manual_force_max{1.0}; + + float _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + matrix::Vector3f _torque_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_rollrate_p, + (ParamFloat) _param_sc_rollrate_i, + (ParamFloat) _param_sc_rr_int_lim, + (ParamFloat) _param_sc_rollrate_d, + (ParamFloat) _param_sc_rollrate_ff, + (ParamFloat) _param_sc_rollrate_k, + + (ParamFloat) _param_sc_pitchrate_p, + (ParamFloat) _param_sc_pitchrate_i, + (ParamFloat) _param_sc_pr_int_lim, + (ParamFloat) _param_sc_pitchrate_d, + (ParamFloat) _param_sc_pitchrate_ff, + (ParamFloat) _param_sc_pitchrate_k, + + (ParamFloat) _param_sc_yawrate_p, + (ParamFloat) _param_sc_yawrate_i, + (ParamFloat) _param_sc_yr_int_lim, + (ParamFloat) _param_sc_yawrate_d, + (ParamFloat) _param_sc_yawrate_ff, + (ParamFloat) _param_sc_yawrate_k, + + (ParamFloat) _param_sc_acro_r_max, + (ParamFloat) _param_sc_acro_p_max, + (ParamFloat) _param_sc_acro_y_max, + (ParamFloat) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamFloat) _param_sc_manual_f_max, + (ParamFloat) _param_sc_manual_t_max, + + (ParamBool) _param_sc_bat_scale_en + ) +}; diff --git a/src/modules/spacecraft/module.yaml b/src/modules/spacecraft/module.yaml deleted file mode 100644 index 5b2b5a264d..0000000000 --- a/src/modules/spacecraft/module.yaml +++ /dev/null @@ -1,235 +0,0 @@ -__max_num_mc_motors: &max_num_mc_motors 12 -__max_num_thrusters: &max_num_thrusters 12 -__max_num_servos: &max_num_servos 8 -__max_num_tilts: &max_num_tilts 4 - -module_name: Control Allocation - -parameters: - - group: Geometry - definitions: - CA_AIRFRAME: - description: - short: Airframe selection - long: | - Defines which mixer implementation to use. - Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators. - - 'Custom' should only be used if noting else can be used. - type: enum - values: - 0: Multirotor - 1: Fixed-wing - 2: Standard VTOL - 3: Tiltrotor VTOL - 4: Tailsitter VTOL - 5: Rover (Ackermann) - 6: Rover (Differential) - 7: Motors (6DOF) - 8: Multirotor with Tilt - 9: Custom - 10: Helicopter (tail ESC) - 11: Helicopter (tail Servo) - 12: Helicopter (Coaxial) - 13: Rover (Mecanum) - 14: Spacecraft 2D - 15: Spacecraft 3D - default: 14 - - CA_METHOD: - description: - short: Control allocation method - long: | - Selects the algorithm and desaturation method. - If set to Automatic, the selection is based on the airframe (CA_AIRFRAME). - type: enum - values: - 0: Pseudo-inverse with output clipping - 1: Pseudo-inverse with metric allocation - 2: Automatic - default: 1 - - CA_R_REV: - description: - short: Bidirectional/Reversible motors - long: | - Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well. - type: bitmask - bit: - 0: Motor 1 - 1: Motor 2 - 2: Motor 3 - 3: Motor 4 - 4: Motor 5 - 5: Motor 6 - 6: Motor 7 - 7: Motor 8 - 8: Motor 9 - 9: Motor 10 - 10: Motor 11 - 11: Motor 12 - default: 0 - - # (SC) Thrusters - CA_THRUSTER_CNT: - description: - short: Total number of thrusters - type: enum - values: - 0: '0' - 1: '1' - 2: '2' - 3: '3' - 4: '4' - 5: '5' - 6: '6' - 7: '7' - 8: '8' - 9: '9' - 10: '10' - 11: '11' - 12: '12' - default: 0 - CA_THRUSTER${i}_PX: - description: - short: Position of thruster ${i} along X body axis - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: 0.0 - CA_THRUSTER${i}_PY: - description: - short: Position of thruster ${i} along Y body axis - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: 0.0 - CA_THRUSTER${i}_PZ: - description: - short: Position of thruster ${i} along Z body axis - type: float - decimal: 2 - increment: 0.1 - unit: m - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: 0.0 - CA_THRUSTER${i}_AX: - description: - short: Axis of thruster ${i} thrust vector, X body axis component - long: Only the direction is considered (the vector is normalized). - type: float - decimal: 2 - increment: 0.1 - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: 0.0 - CA_THRUSTER${i}_AY: - description: - short: Axis of thruster ${i} thrust vector, Y body axis component - long: Only the direction is considered (the vector is normalized). - type: float - decimal: 2 - increment: 0.1 - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: 0.0 - CA_THRUSTER${i}_AZ: - description: - short: Axis of thruster ${i} thrust vector, Z body axis component - long: Only the direction is considered (the vector is normalized). - type: float - decimal: 2 - increment: 0.1 - num_instances: *max_num_thrusters - min: -100 - max: 100 - default: -1.0 - - CA_THRUSTER${i}_CT: - description: - short: Thrust coefficient of rotor ${i} - long: | - The thrust coefficient if defined as Thrust = CT * u^2, - where u (with value between actuator minimum and maximum) - is the output signal sent to the motor controller. - type: float - decimal: 1 - increment: 1 - num_instances: *max_num_thrusters - min: 0 - max: 100 - default: 6.5 - -# Mixer -mixer: - actuator_types: - motor: - functions: 'Motor' - actuator_testing_values: - min: 0 - max: 1 - default_is_nan: true - DEFAULT: - actuator_testing_values: - min: -1 - max: 1 - default: -1 - - config: - param: CA_AIRFRAME - types: - 14: # Spacecraft 2D - title: 'Spacecraft 2D' - actuators: - - actuator_type: 'motor' - count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size - per_item_parameters: - standard: - position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] - extra: - - name: 'CA_THRUSTER${i}_AX' - label: 'Axis X' - function: 'axisx' - advanced: true - - name: 'CA_THRUSTER${i}_AY' - label: 'Axis Y' - function: 'axisy' - advanced: true - - name: 'CA_THRUSTER${i}_AZ' - label: 'Axis Z' - function: 'axisz' - advanced: true - - 15: # Sapcecraft 3D - title: 'Spacecraft 3D' - actuators: - - actuator_type: 'motor' - count: 'CA_THRUSTER_CNT' - per_item_parameters: - standard: - position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] - extra: - - name: 'CA_THRUSTER${i}_AX' - label: 'Axis X' - function: 'axisx' - advanced: true - - name: 'CA_THRUSTER${i}_AY' - label: 'Axis Y' - function: 'axisy' - advanced: true - - name: 'CA_THRUSTER${i}_AZ' - label: 'Axis Z' - function: 'axisz' - advanced: true diff --git a/src/modules/spacecraft/spacecraft_attitude_params.c b/src/modules/spacecraft/spacecraft_attitude_params.c new file mode 100644 index 0000000000..6e2c27b192 --- /dev/null +++ b/src/modules/spacecraft/spacecraft_attitude_params.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sc_att_control_params.c + * Parameters for spacecraft attitude controller. + * + * @author Pedro Roque, + */ + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f); + +/** + * Yaw weight + * + * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control. + * Deprioritizing yaw is necessary because multicopters have much less control authority + * in yaw compared to the other axes and it makes sense because yaw is not critical for + * stable hovering or 3D navigation. + * + * For yaw control tuning use SC_YAW_P. This ratio has no impact on the yaw gain. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f); + +/** + * Max roll rate + * + * Limit for roll rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f); + +/** + * Max yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f); + +/** + * Manual tilt input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); + +/** + * Maximal tilt angle in Stabilized or Manual mode + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_MAX, 90.f); diff --git a/src/modules/spacecraft/spacecraft_position_params.c b/src/modules/spacecraft/spacecraft_position_params.c new file mode 100644 index 0000000000..6b9065f8e6 --- /dev/null +++ b/src/modules/spacecraft/spacecraft_position_params.c @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Proportional gain for position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_P, 0.2f); + +/** + * Integral gain for position error + * + * Defined as corrective velocity in m/s per m velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_I, 0.f); + +/** + * Integral limit for position error + * + * Defined as corrective velocity in m/s per m velocity error + * + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_I_LIM, 1.f); + +/** + * Proportional gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_P, 6.55f); + +/** + * Integral gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I, 0.f); + +/** + * Integral limit for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I_LIM, 1.f); + +/** + * Derivative gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0.0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_D, 0.0f); + +/** + * Maximum velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MAX, 12.f); + +/** + * Overall Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_ALL, -10.f); + +/** + * Cruising elocity setpoint in autonomous modes + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_CRUISE, 10.f); + +/** + * Maximum velocity setpoint in Position mode + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MANUAL, 10.f); + +/** + * Maximum collective thrust + * + * Limit allowed thrust + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_THR_MAX, 1.f); + +/** + * Acceleration for autonomous and for manual modes + * + * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC, 3.f); + +/** + * Maximum accelaration in autonomous modes + * + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f); + +PARAM_DEFINE_FLOAT(SPC_VEHICLE_RESP, 0.5f); +/** + * Maximum jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_MAX, 8.f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_MAX, 150.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VELD_LP, 5.0f); diff --git a/src/modules/spacecraft/spacecraft_rate_params.c b/src/modules/spacecraft/spacecraft_rate_params.c new file mode 100644 index 0000000000..4afb818a42 --- /dev/null +++ b/src/modules/spacecraft/spacecraft_rate_params.c @@ -0,0 +1,421 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2025 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 spacecraft_params.c + * Parameters for spacecraft vehicle type. + * + * @author Pedro Roque + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error + * + SC_ROLLRATE_I * error_integral + * + SC_ROLLRATE_D * error_derivative) + * Set SC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set SC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error + * + SC_PITCHRATE_I * error_integral + * + SC_PITCHRATE_D * error_derivative) + * Set SC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set SC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f); + +/** + * Yaw rate integrator limit + * + * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_YAWRATE_K * (SC_YAWRATE_P * error + * + SC_YAWRATE_I * error_integral + * + SC_YAWRATE_D * error_derivative) + * Set SC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set SC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); + +/** + * Manual mode maximum force. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f); + +/** + * Manual mode maximum torque. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Spacecraft Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);