mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 02:57:34 +08:00
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 <alexander@auterion.com>
This commit is contained in:
@@ -0,0 +1,145 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name 3DoF Spacecraft Model
|
||||
#
|
||||
# @type 2D Freeflyer with 8 thrusters - Planar motion
|
||||
#
|
||||
# @maintainer Pedro Roque <padr@kth.se>
|
||||
#
|
||||
|
||||
. ${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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -69,7 +69,7 @@ then
|
||||
fi
|
||||
|
||||
#
|
||||
# Spapcecraft setup.
|
||||
# Spacecraft setup.
|
||||
#
|
||||
if [ $VEHICLE_TYPE = spacecraft ]
|
||||
then
|
||||
|
||||
Reference in New Issue
Block a user