diff --git a/.gitignore b/.gitignore index 029a45346f..4aa1d72166 100644 --- a/.gitignore +++ b/.gitignore @@ -51,3 +51,4 @@ xcode src/platforms/posix/px4_messages/ src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ +ROMFS/*/*/rc.autostart diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 1e3dc6c5f6..7938c47bae 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,7 +1,15 @@ #!nsh # -# HILStar -# +# @name HILStar (XPlane) +# +# @type Simulation +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 rudder +# @output MAIN4 throttle +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 0dd4837644..2058d10091 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,8 +1,10 @@ #!nsh # -# Team Blacksheep Discovery Quadcopter +# @name Team Blacksheep Discovery # -# Anton Babushkin , Simon Wilks +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 68428be6f6..a2536e5418 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,8 +1,10 @@ #!nsh # -# 3DR Iris Quadcopter +# @name 3DR Iris Quadrotor # -# Anton Babushkin +# @type Quadrotor Wide +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 36d387c759..7e27043156 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -1,8 +1,10 @@ #!nsh # -# Steadidrone QU4D +# @name Steadidrone QU4D # -# Thomas Gubler +# @type Quadrotor Wide +# +# @maintainer Thomas Gubler # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index e83a864d8d..e26e2a8105 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -1,10 +1,10 @@ #!nsh # -# Team Blacksheep Discovery Long Range Quadcopter -# -# Setup: 15 x 5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors +# @name Team Blacksheep Discovery Endurance # -# Simon Wilks +# @type Quadrotor Wide +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index b91de228c5..4f22ddc436 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -1,8 +1,10 @@ #!nsh # -# HobbyKing SK450 DeadCat modification +# @name HobbyKing SK450 DeadCat modification # -# Anton Matosov +# @type Quadrotor Wide +# +# @maintainer Anton Matosov # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 03b6b30d27..e1f2cdfe8d 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Quadcopter X +# @name HIL Quadcopter X # -# Anton Babushkin +# @type Simulation +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad index 0dbffba00f..26509d128d 100644 --- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -1,32 +1,25 @@ #!nsh # -# @name 3DR Iris Quadrotor +# @name 3DR DIY Quad # -# @type Quadrotor Wide +# @type Quadrotor x # -# @maintainer Anton Babushkin +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then - # TODO tune roll/pitch separately - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.05 + param set MC_ROLL_P 6.5 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.1 param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.05 + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_I 0.09 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.5 - param set MC_YAWRATE_P 0.25 - param set MC_YAWRATE_I 0.25 - param set MC_YAWRATE_D 0.0 - - param set BAT_V_SCALING 0.00989 - param set BAT_C_SCALING 0.0124 + param set MC_YAW_P 4 fi set MIXER quad_x diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 7e651216d0..7aa888c763 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Quadcopter + +# @name HIL Quadcopter + # -# Anton Babushkin +# @type Simulation +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 0909208478..57bcd24d02 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Rascal 110 (Flightgear) +# @name HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# @type Simulation +# +# @maintainer Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 15e5cf21d8..5e0b6cd746 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -1,8 +1,10 @@ #!nsh # -# HIL Malolo 1 (Flightgear) +# @name HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# @type Simulation +# +# @maintainer Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 0bb8cb52ee..f85b06f87d 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -1,8 +1,10 @@ #!nsh # -# Generic 10" Hexa coaxial geometry +# @name Generic Hexa coaxial geometry # -# Lorenz Meier +# @type Hexarotor Coaxial +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 16e86fd5fa..431d883155 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -1,8 +1,10 @@ #!nsh # -# Generic 10" Octo coaxial geometry +# @name Generic 10" Octo coaxial geometry # -# Lorenz Meier +# @type Octorotor Coaxial +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 912202f962..a9e7ff35b2 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,7 +1,10 @@ +#!nsh # -# Generic configuration file for caipirinha VTOL version +# @name Duorotor Tailsitter # -# Roman Bapst +# @type VTOL Tailsitter +# +# @maintainer Roman Bapst # sh /etc/init.d/rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 963341d138..1ec65dff3e 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -1,8 +1,10 @@ #!nsh # -# Generic configuration file for BirdsEyeView Aerobotics FireFly6 +# @name BirdsEyeView Aerobotics FireFly6 # -# Roman Bapst +# @type VTOL Tiltrotor +# +# @maintainer Roman Bapst # sh /etc/init.d/rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter index 17560526a3..016f13816f 100644 --- a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -1,7 +1,10 @@ +#!nsh # -# Generic configuration file for a tailsitter with motors in X configuration. +# @name Quadrotor X Tailsitter # -# Roman Bapst +# @type VTOL Tailsitter +# +# @maintainer Roman Bapst # sh /etc/init.d/rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter new file mode 100644 index 0000000000..dcf2fc8679 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter @@ -0,0 +1,21 @@ +#!nsh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Tailsitter +# +# @maintainer Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER quad_+_vtol + +set PWM_OUT 1234 +set PWM_MAX 2000 +set PWM_RATE 400 +set MAV_TYPE 20 +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 +param set VT_ELEV_MC_LOCK 1 diff --git a/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ b/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ index 3bb68ecf3f..b4c7885dab 100644 --- a/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ +++ b/ROMFS/px4fmu_common/init.d/14001_tri_y_yaw+ @@ -1,9 +1,10 @@ #!nsh # -# Generic Tricopter Y Geometry -# Yaw Servo +Output ==> +Yaw Vehicle Rotation +# @name Generic Tricopter Y+ Geometry # -# Trent Lukaczyk +# @type Tricopter Y+ +# +# @maintainer Trent Lukaczyk # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- b/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- index fd4d476476..7fa0af009a 100644 --- a/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- +++ b/ROMFS/px4fmu_common/init.d/14002_tri_y_yaw- @@ -1,9 +1,10 @@ #!nsh # -# Generic Tricopter Y Geometry -# Yaw Servo +Output ==> -Yaw Vehicle Rotation +# @name Generic Tricopter Y- Geometry # -# Trent Lukaczyk +# @type Tricopter Y- +# +# @maintainer Trent Lukaczyk # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 3ab2ac3d1a..12c12aec92 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,6 +1,10 @@ #!nsh # -# MPX EasyStar Plane +# @name Multiplex Easystar +# +# @type Standard Plane +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 05ee57ffa0..19a47cda8c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -1,4 +1,21 @@ #!nsh +# +# @name Standard AERT Plane +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN4 rudder +# @output MAIN3 throttle +# @output MAIN5 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 906f4b1ccf..d2347c98c8 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,4 +1,21 @@ #!nsh +# +# @name Skywalker (3DR Aero) +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN4 rudder +# @output MAIN3 throttle +# @output MAIN5 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 index 2433ab4f40..133ffe5722 100644 --- a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 +++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 @@ -1,4 +1,19 @@ #!nsh +# +# @name Skyhunter 1800 +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index bb4390b1d3..083eb42330 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -1,4 +1,21 @@ #!nsh +# +# @name Standard AETR Plane +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 throttle +# @output MAIN4 rudder +# @output MAIN5 flaps +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 040b78dc77..a15d0ac6d0 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,4 +1,21 @@ #!nsh +# +# @name IO Camflyer +# +# @url https://pixhawk.org/platforms/planes/bormatec_camflyer_q +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Simon Wilks +# sh /etc/init.d/rc.fw_defaults @@ -17,11 +34,9 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.005 param set FW_PR_IMAX 0.4 param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 fi diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index c7dd1dfc5c..84d9a9f39e 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -1,8 +1,20 @@ #!nsh # -# Phantom FPV Flying Wing +# @name Phantom FPV Flying Wing # -# Simon Wilks +# @url https://pixhawk.org/platforms/planes/z-84_wing_wing +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -16,14 +28,12 @@ then param set FW_L1_DAMPING 0.75 param set FW_L1_PERIOD 15 param set FW_PR_FF 0.2 - param set FW_PR_I 0.005 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 - param set FW_RR_I 0.02 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.08 param set FW_R_LIM 50 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 4950c3183f..3dedf8170d 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,8 +1,20 @@ #!nsh # -# Skywalker X5 Flying Wing +# @name Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# @url https://pixhawk.org/platforms/planes/skywalker_x5 +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 3c2312fc7d..25a14312f3 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -1,8 +1,20 @@ #!nsh # -# Wing Wing (aka Z-84) Flying Wing +# @name Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# @url https://pixhawk.org/platforms/planes/z-84_wing_wing +# +# @type Flying Wing +# +# @output MAIN1 left aileron +# @output MAIN2 right aileron +# @output MAIN4 throttle +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index 9f45c636b0..353da2ffc4 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -1,8 +1,10 @@ #!nsh # -# FX-79 Buffalo Flying Wing +# @name FX-79 Buffalo Flying Wing # -# Simon Wilks +# @type Flying Wing +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 0f5f5502aa..c216abf7b2 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -1,8 +1,11 @@ +# #!nsh # -# Viper +# @name Viper # -# Simon Wilks +# @type Flying Wing +# +# @maintainer Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 8a661f25e2..5aa004db1b 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,8 +1,10 @@ #!nsh # -# TBS Caipirinha +# @name TBS Caipirinha # -# Lorenz Meier +# @type Flying Wing +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -22,16 +24,17 @@ then param set FW_LND_TLALT 5 param set FW_THR_LND_MAX 0 param set FW_PR_FF 0.35 - param set FW_PR_I 0.01 param set FW_PR_IMAX 0.4 param set FW_PR_P 0.08 param set FW_RR_FF 0.6 - param set FW_RR_I 0.01 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 param set SYS_COMPANION 157600 param set PWM_MAIN_REV0 1 param set PWM_MAIN_REV1 1 + param set PWM_DISARMED 0 + param set PWM_MIN 900 + param set PWM_MAX 2100 fi set PWM_DISARMED p:PWM_DISARMED diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index e0538160f0..8f7c8da356 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Quad X geometry +# @name Generic Quadrotor X config # -# Lorenz Meier +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index c3358ef4c8..9d3e4427be 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -1,6 +1,10 @@ #!nsh # -# ARDrone +# @name AR.Drone Frame +# +# @type Quadrotor x +# +# @maintainer Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 512ad132be..4303320ffe 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,14 @@ #!nsh # -# DJI Flame Wheel F330 +# @name DJI Flame Wheel F330 # -# Anton Babushkin +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/4001_quad_x @@ -21,7 +27,12 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + # DJI ESCs do not support calibration and need a higher min + param set PWM_MIN 1230 fi -set PWM_MIN 1200 -set PWM_MAX 1950 +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1075 +then + param set PWM_MIN 1230 +fi diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 2a77f13866..55e5237945 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,14 @@ #!nsh # -# DJI Flame Wheel F450 +# @name DJI Flame Wheel F450 # -# Lorenz Meier +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier # sh /etc/init.d/4001_quad_x @@ -22,7 +28,12 @@ then param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + # DJI ESCs do not support calibration and need a higher min + param set PWM_MIN 1230 fi -set PWM_MIN 1230 -set PWM_MAX 1950 +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1075 +then + param set PWM_MIN 1230 +fi diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index c6341a4f74..1ffffea22b 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -1,8 +1,10 @@ #!nsh # -# F450-sized quadrotor with CAN +# @name F450-sized quadrotor with CAN # -# Pavel Kirienko +# @type Quadrotor x +# +# @maintainer Pavel Kirienko # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index aec0c62f8f..a6a31a30a9 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -1,11 +1,11 @@ #!nsh # -# Hobbyking Micro Integrated PCB Quadcopter -# with SimonK ESC firmware and Mystery A1510 motors +# @name Hobbyking Micro PCB # -# Thomas Gubler +# @type Quadrotor x +# +# @maintainer Thomas Gubler # -echo "HK Micro PCB Quad" sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 index a36bac5cc8..b3e7f0978e 100644 --- a/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 +++ b/ROMFS/px4fmu_common/init.d/50001_axialracing_ax10 @@ -1,9 +1,10 @@ #!nsh # -# loading default values for the axialracing ax10 +# @name Axial Racing AX10 +# +# @type Rover # -#load some defaults e.g. PWM values sh /etc/init.d/rc.axialracing_ax10_defaults #choose a mixer, for rover control we need a plain passthrough to the servos diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index e51f9cf89d..8c5eed28cd 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -1,8 +1,10 @@ #!nsh # -# Generic 10" Quad + geometry +# @name Generic 10" Quad + geometry # -# Anton Babushkin +# @type Quadrotor + +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 7a6dda648a..584c4a381c 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Hexa X geometry +# @name Generic Hexarotor x geometry # -# Anton Babushkin +# @type Hexarotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index dd9589d614..11d732471a 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Hexa + geometry +# @name Generic Hexarotor + geometry # -# Anton Babushkin +# @type Hexarotor + +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 7cbb3ddfcd..272e093550 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Octo X geometry +# @name Generic Octocopter X geometry # -# Anton Babushkin +# @type Octorotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 5d608d593c..d0db57cf17 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -1,8 +1,14 @@ #!nsh # -# Generic 10" Octo + geometry +# @name Generic Octocopter + geometry # -# Anton Babushkin +# @type Octorotor + +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart deleted file mode 100644 index ec405accac..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ /dev/null @@ -1,310 +0,0 @@ -# -# SYS_AUTOSTART = 0 means no autostart (default) -# -# AUTOSTART PARTITION: -# 0 .. 999 Reserved (historical) -# 1000 .. 1999 Simulation setups -# 2000 .. 2999 Standard planes -# 3000 .. 3999 Flying wing -# 4000 .. 4999 Quad X -# 5000 .. 5999 Quad + -# 6000 .. 6999 Hexa X -# 7000 .. 7999 Hexa + -# 8000 .. 8999 Octo X -# 9000 .. 9999 Octo + -# 10000 .. 10999 Wide arm / H frame -# 11000 .. 11999 Hexa Cox -# 12000 .. 12999 Octo Cox -# 13000 .. 13999 VTOL -# 14000 .. 14999 Tri Y - -# -# Simulation -# - -if param compare SYS_AUTOSTART 901 -then - sh /etc/init.d/901_bottle_drop_test.hil - set MODE custom -fi - -if param compare SYS_AUTOSTART 1000 -then - sh /etc/init.d/1000_rc_fw_easystar.hil -fi - -if param compare SYS_AUTOSTART 1001 -then - sh /etc/init.d/1001_rc_quad_x.hil -fi - -if param compare SYS_AUTOSTART 1003 -then - sh /etc/init.d/1003_rc_quad_+.hil -fi - -if param compare SYS_AUTOSTART 1004 -then - sh /etc/init.d/1004_rc_fw_Rascal110.hil -fi - -if param compare SYS_AUTOSTART 1005 -then - sh /etc/init.d/1005_rc_fw_Malolo1.hil -fi - -# -# Plane -# - -if param compare SYS_AUTOSTART 2100 100 -then - sh /etc/init.d/2100_mpx_easystar - set MODE custom -fi - -if param compare SYS_AUTOSTART 2101 101 -then - sh /etc/init.d/2101_fw_AERT - set MODE custom -fi - -if param compare SYS_AUTOSTART 2102 102 -then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom -fi - -if param compare SYS_AUTOSTART 2103 103 -then - sh /etc/init.d/2103_skyhunter_1800 - set MODE custom -fi - -if param compare SYS_AUTOSTART 2104 -then - sh /etc/init.d/2104_fw_AETR - set MODE custom -fi - -# -# Flying wing -# - -if param compare SYS_AUTOSTART 3030 30 -then - sh /etc/init.d/3030_io_camflyer -fi - -if param compare SYS_AUTOSTART 3031 31 -then - sh /etc/init.d/3031_phantom -fi - -if param compare SYS_AUTOSTART 3032 32 -then - sh /etc/init.d/3032_skywalker_x5 -fi - -if param compare SYS_AUTOSTART 3033 33 -then - sh /etc/init.d/3033_wingwing -fi - -if param compare SYS_AUTOSTART 3034 34 -then - sh /etc/init.d/3034_fx79 -fi - -if param compare SYS_AUTOSTART 3035 35 -then - sh /etc/init.d/3035_viper -fi - -if param compare SYS_AUTOSTART 3100 -then - sh /etc/init.d/3100_tbs_caipirinha -fi - -# -# Quad X -# - -if param compare SYS_AUTOSTART 4001 -then - sh /etc/init.d/4001_quad_x -fi - -# -# ARDrone -# - -if param compare SYS_AUTOSTART 4008 8 -then - sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4010 10 -then - sh /etc/init.d/4010_dji_f330 -fi - -if param compare SYS_AUTOSTART 4011 11 -then - sh /etc/init.d/4011_dji_f450 -fi - -if param compare SYS_AUTOSTART 4012 -then - sh /etc/init.d/4012_quad_x_can -fi - -if param compare SYS_AUTOSTART 4020 -then - sh /etc/init.d/4020_hk_micro_pcb -fi - -# -# Quad + -# - -if param compare SYS_AUTOSTART 5001 -then - sh /etc/init.d/5001_quad_+ -fi - -# -# Hexa X -# - -if param compare SYS_AUTOSTART 6001 -then - sh /etc/init.d/6001_hexa_x -fi - -# -# Hexa + -# - -if param compare SYS_AUTOSTART 7001 -then - sh /etc/init.d/7001_hexa_+ -fi - -# -# Octo X -# - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x -fi - -# -# Octo + -# - -if param compare SYS_AUTOSTART 9001 -then - sh /etc/init.d/9001_octo_+ -fi - -# -# Wide arm / H frame -# - -if param compare SYS_AUTOSTART 10015 15 -then - sh /etc/init.d/10015_tbs_discovery -fi - -if param compare SYS_AUTOSTART 10016 16 -then - sh /etc/init.d/10016_3dr_iris -fi - -if param compare SYS_AUTOSTART 10017 -then - sh /etc/init.d/10017_steadidrone_qu4d -fi - -if param compare SYS_AUTOSTART 10018 18 -then - sh /etc/init.d/10018_tbs_endurance -fi - -if param compare SYS_AUTOSTART 10019 -then - sh /etc/init.d/10019_sk450_deadcat -fi - -# -# Hexa Coaxial -# - -if param compare SYS_AUTOSTART 11001 -then - sh /etc/init.d/11001_hexa_cox -fi - -# -# Octo Coaxial -# - -if param compare SYS_AUTOSTART 12001 -then - sh /etc/init.d/12001_octo_cox -fi - -# 13000 is historically reserved for the quadshot - -# -# VTOL Caipririnha (Tailsitter) -# -if param compare SYS_AUTOSTART 13001 -then - sh /etc/init.d/13001_caipirinha_vtol -fi - -# -# VTOL BirdsEyeView FireFly x6 (Tilt-Rotor) -# -if param compare SYS_AUTOSTART 13002 -then - sh /etc/init.d/13002_firefly6 -fi - -# -# Tailsitter with 4 motors in x config -# -if param compare SYS_AUTOSTART 13003 -then - sh /etc/init.d/13003_quad_tailsitter -fi - -# -# TriCopter Y Yaw+ -# -if param compare SYS_AUTOSTART 14001 -then - sh /etc/init.d/14001_tri_y_yaw+ -fi - -# -# TriCopter Y Yaw- -# -if param compare SYS_AUTOSTART 14002 -then - sh /etc/init.d/14002_tri_y_yaw- -fi - - - -# -# Ground Rover AxialRacing AX10 -# -if param compare SYS_AUTOSTART 50001 -then - sh /etc/init.d/50001_axialracing_ax10 -fi - diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 6506ed8c3e..eccee07e7b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -14,12 +14,31 @@ then param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 + param set PWM_DISARMED 900 + param set PWM_MIN 1075 + param set PWM_MAX 1950 fi +# Transitional support: ensure suitable PWM min/max param values +if param compare PWM_MIN 1000 +then + param set PWM_MIN 1075 +fi +if param compare PWM_MAX 2000 +then + param set PWM_MAX 1950 +fi +if param compare PWM_DISARMED 0 +then + param set PWM_DISARMED 900 +fi + +# set environment variables (!= parameters) set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1075 -set PWM_MAX 2000 +# tell the mixer to use parameters for these instead +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX # This is the gimbal pass mixer set MIXER_AUX pass diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4ca341372..db142c30b8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -575,7 +575,7 @@ then fi if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ] then - set MAV_TYPE 2 + set MAV_TYPE 15 fi if [ $MIXER == hexa_x -o $MIXER == hexa_+ ] then @@ -599,6 +599,7 @@ then if [ $MAV_TYPE == none ] then echo "Unknown MAV_TYPE" + param set MAV_TYPE 2 else param set MAV_TYPE $MAV_TYPE fi @@ -642,6 +643,7 @@ then if [ $MAV_TYPE == none ] then echo "Unknown MAV_TYPE" + param set MAV_TYPE 19 else param set MAV_TYPE $MAV_TYPE fi @@ -672,6 +674,8 @@ then then sh /etc/init.d/rc.axialracing_ax10_apps fi + + param set MAV_TYPE 10 fi unset MIXER @@ -708,7 +712,7 @@ then fi unset EXIT_ON_END - # Run no SD alarm last + # Run no SD alarm if [ $LOG_FILE == /dev/null ] then echo "[i] No microSD card found" @@ -716,6 +720,9 @@ then tone_alarm error fi + # Boot is complete, inform MAVLink app(s) that the system is now fully up and running + mavlink boot_complete + # End of autostart fi diff --git a/ROMFS/px4fmu_common/mixers/quad_+_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_+_vtol.main.mix new file mode 100644 index 0000000000..3021221d80 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_+_vtol.main.mix @@ -0,0 +1,28 @@ +Mixer for Tailsitter with + motor configuration and elevons +=========================================================== + +This file defines a single mixer for tailsitter with motors in X configuration. All controls +are mixed 100%. + +R: 4+ 10000 10000 10000 0 + +# mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 + +# mixer for canard surface +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 + +# mixer for rudder +M: 1 +O: 10000 10000 0 -10000 10000 +S: 1 2 -10000 -10000 0 -10000 10000 diff --git a/Tools/px4airframes/README.md b/Tools/px4airframes/README.md new file mode 100644 index 0000000000..50dcd2e296 --- /dev/null +++ b/Tools/px4airframes/README.md @@ -0,0 +1 @@ +This folder contains a python library used by px_process_params.py diff --git a/Tools/px4airframes/__init__.py b/Tools/px4airframes/__init__.py new file mode 100644 index 0000000000..43e82d2643 --- /dev/null +++ b/Tools/px4airframes/__init__.py @@ -0,0 +1 @@ +__all__ = ["srcscanner", "srcparser", "xmlout", "rcout"] \ No newline at end of file diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py new file mode 100644 index 0000000000..de5af0f305 --- /dev/null +++ b/Tools/px4airframes/rcout.py @@ -0,0 +1,54 @@ +from xml.sax.saxutils import escape +import codecs + +class RCOutput(): + def __init__(self, groups, board): + result = ( "#\n" + "#\n" + "# THIS FILE IS AUTO-GENERATED. DO NOT EDIT!\n" + "#\n" + "#\n" + "# SYS_AUTOSTART = 0 means no autostart (default)\n" + "#\n" + "# AUTOSTART PARTITION:\n" + "# 0 .. 999 Reserved (historical)\n" + "# 1000 .. 1999 Simulation setups\n" + "# 2000 .. 2999 Standard planes\n" + "# 3000 .. 3999 Flying wing\n" + "# 4000 .. 4999 Quadrotor x\n" + "# 5000 .. 5999 Quadrotor +\n" + "# 6000 .. 6999 Hexarotor x\n" + "# 7000 .. 7999 Hexarotor +\n" + "# 8000 .. 8999 Octorotor x\n" + "# 9000 .. 9999 Octorotor +\n" + "# 10000 .. 10999 Quadrotor Wide arm / H frame\n" + "# 11000 .. 11999 Hexa Cox\n" + "# 12000 .. 12999 Octo Cox\n" + "# 13000 .. 13999 VTOL\n" + "# 14000 .. 14999 Tri Y\n" + "\n") + for group in groups: + result += "# GROUP: %s\n\n" % group.GetName() + for param in group.GetParams(): + path = param.GetPath().rsplit('/', 1)[1] + id_val = param.GetId() + name = param.GetFieldValue("short_desc") + long_desc = param.GetFieldValue("long_desc") + + result += "#\n" + result += "# %s\n" % param.GetName() + result += "if param compare SYS_AUTOSTART %s\n" % id_val + result += "then\n" + result += "\tsh /etc/init.d/%s\n" % path + result += "fi\n" + + #if long_desc is not None: + # result += "# %s\n" % long_desc + result += "\n" + + result += "\n" + self.output = result; + + def Save(self, filename): + with codecs.open(filename, 'w', 'utf-8') as f: + f.write(self.output) diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py new file mode 100644 index 0000000000..699a2cdc3b --- /dev/null +++ b/Tools/px4airframes/srcparser.py @@ -0,0 +1,346 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + key=lambda x: x.GetFieldValue("code")) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "board": 9, + "short_desc": 8, + "long_desc": 7, + "min": 5, + "max": 4, + "unit": 3, + # all others == 0 (sorted alphabetically) + } + + def __init__(self, path, name, airframe_type, airframe_id, maintainer): + self.fields = {} + self.outputs = {} + self.path = path + self.name = name + self.type = airframe_type + self.id = airframe_id + self.maintainer = maintainer + + def GetPath(self): + return self.path + + def GetName(self): + return self.name + + def GetType(self): + return self.type + + def GetId(self): + return self.id + + def GetMaintainer(self): + return self.maintainer + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def SetOutput(self, code, value): + """ + Set named output value + """ + self.outputs[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + keys = self.fields.keys() + keys = sorted(keys) + keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True) + return keys + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + fv = self.fields.get(code) + if not fv: + # required because python 3 sorted does not accept None + return "" + return self.fields.get(code) + + def GetOutputCodes(self): + """ + Return list of existing output codes in convenient order + """ + keys = self.outputs.keys() + keys = sorted(keys) + keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True) + return keys + + def GetOutputValue(self, code): + """ + Return value of the given output code or None if not found. + """ + fv = self.outputs.get(code) + if not fv: + # required because python 3 sorted does not accept None + return "" + return self.outputs.get(code) + +class SourceParser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\#\s') + re_comment_content = re.compile(r'^\#\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\#\n/') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + re_remove_carriage_return = re.compile('\n+') + + valid_tags = set(["url", "maintainer", "output", "name", "type"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. The parser uses any extension. + """ + return [""] + + def Parse(self, path, contents): + """ + Incrementally parse program contents and append all found airframes + to the list. + """ + + airframe_id = None + airframe_id = path.rsplit('/',1)[1].split('_',1)[0] + + # Skip if not numeric + if (not self.IsNumber(airframe_id)): + return True + + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + tags = {} + outputs = {} + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if state is None and self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + if state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + if (tag == "output"): + key, text = desc.split(' ', 1) + outputs[key] = text; + else: + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + state = None + + # Process parsed content + airframe_type = None + maintainer = "John Doe " + airframe_name = None + + # Done with file, store + for tag in tags: + if tag == "maintainer": + maintainer = tags[tag] + elif tag == "type": + airframe_type = tags[tag] + elif tag == "name": + airframe_name = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Aborting due to invalid documentation tag: '%s'\n" % tag) + return False + + # Sanity check + if airframe_type == None: + sys.stderr.write("Aborting due to missing @type tag in file: '%s'\n" % path) + return False + + if airframe_name == None: + sys.stderr.write("Aborting due to missing @name tag in file: '%s'\n" % path) + return False + + # We already know this is an airframe config, so add it + param = Parameter(path, airframe_name, airframe_type, airframe_id, maintainer) + + # Done with file, store + for tag in tags: + if tag == "maintainer": + maintainer = tags[tag] + if tag == "type": + airframe_type = tags[tag] + if tag == "name": + airframe_name = tags[tag] + else: + param.SetField(tag, tags[tag]) + + # Store outputs + for output in outputs: + param.SetOutput(output, outputs[output]) + + # Store the parameter + if airframe_type not in self.param_groups: + self.param_groups[airframe_type] = ParameterGroup(airframe_type) + self.param_groups[airframe_type].AddParameter(param) + + return True + + def IsNumber(self, numberString): + try: + float(numberString) + return True + except ValueError: + return False + + def Validate(self): + """ + Validates the airframe meta data. + """ + seenParamNames = [] + for group in self.GetParamGroups(): + for param in group.GetParams(): + name = param.GetName() + board = param.GetFieldValue("board") + # Check for duplicates + name_plus_board = name + "+" + board + for seenParamName in seenParamNames: + if seenParamName == name_plus_board: + sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board)) + return False + seenParamNames.append(name_plus_board) + # Validate values + default = param.GetDefault() + min = param.GetFieldValue("min") + max = param.GetFieldValue("max") + #sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max)) + if default != "" and not self.IsNumber(default): + sys.stderr.write("Default value not number: {0} {1}\n".format(name, default)) + return False + if min != "": + if not self.IsNumber(min): + sys.stderr.write("Min value not number: {0} {1}\n".format(name, min)) + return False + if default != "" and float(default) < float(min): + sys.stderr.write("Default value is smaller than min: {0} default:{1} min:{2}\n".format(name, default, min)) + return False + if max != "": + if not self.IsNumber(max): + sys.stderr.write("Max value not number: {0} {1}\n".format(name, max)) + return False + if default != "" and float(default) > float(max): + sys.stderr.write("Default value is larger than max: {0} default:{1} max:{2}\n".format(name, default, max)) + return False + return True + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + groups = self.param_groups.values() + groups = sorted(groups, key=lambda x: x.GetName()) + groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True) + return groups diff --git a/Tools/px4airframes/srcscanner.py b/Tools/px4airframes/srcscanner.py new file mode 100644 index 0000000000..c1f280668d --- /dev/null +++ b/Tools/px4airframes/srcscanner.py @@ -0,0 +1,37 @@ +import os +import re +import codecs + +class SourceScanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = tuple(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + if filename.endswith(extensions): + path = os.path.join(dirname, filename) + if not self.ScanFile(path, parser): + return False + return True + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with codecs.open(path, 'r', 'utf-8') as f: + try: + contents = f.read() + except: + contents = '' + print('Failed reading file: %s, skipping content.' % path) + pass + return parser.Parse(path, contents) diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py new file mode 100644 index 0000000000..427b1090a2 --- /dev/null +++ b/Tools/px4airframes/xmlout.py @@ -0,0 +1,84 @@ +import xml.etree.ElementTree as ET +import codecs + +def indent(elem, level=0): + i = "\n" + level*" " + if len(elem): + if not elem.text or not elem.text.strip(): + elem.text = i + " " + if not elem.tail or not elem.tail.strip(): + elem.tail = i + for elem in elem: + indent(elem, level+1) + if not elem.tail or not elem.tail.strip(): + elem.tail = i + else: + if level and (not elem.tail or not elem.tail.strip()): + elem.tail = i + +class XMLOutput(): + + def __init__(self, groups, board): + xml_parameters = ET.Element("airframes") + xml_version = ET.SubElement(xml_parameters, "version") + xml_version.text = "1" + last_param_name = "" + board_specific_param_set = False + for group in groups: + xml_group = ET.SubElement(xml_parameters, "airframe_group") + xml_group.attrib["name"] = group.GetName() + if (group.GetName() == "Standard Plane"): + xml_group.attrib["image"] = "AirframeStandardPlane.png" + elif (group.GetName() == "Flying Wing"): + xml_group.attrib["image"] = "AirframeFlyingWing.png" + elif (group.GetName() == "Quadrotor x"): + xml_group.attrib["image"] = "AirframeQuadRotorX.png" + elif (group.GetName() == "Quadrotor +"): + xml_group.attrib["image"] = "AirframeQuadRotorPlus.png" + elif (group.GetName() == "Hexarotor x"): + xml_group.attrib["image"] = "AirframeHexaRotorX.png" + elif (group.GetName() == "Hexarotor +"): + xml_group.attrib["image"] = "AirframeHexaRotorPlus.png" + elif (group.GetName() == "Octorotor +"): + xml_group.attrib["image"] = "AirframeOctoRotorPlus.png" + elif (group.GetName() == "Octorotor x"): + xml_group.attrib["image"] = "AirframeOctoRotorX.png" + elif (group.GetName() == "Quadrotor Wide"): + xml_group.attrib["image"] = "AirframeQuadRotorH.png" + elif (group.GetName() == "Quadrotor H"): + xml_group.attrib["image"] = "AirframeQuadRotorH.png" + elif (group.GetName() == "Simulation"): + xml_group.attrib["image"] = "AirframeSimulation.png" + else: + xml_group.attrib["image"] = "" + for param in group.GetParams(): + if (last_param_name == param.GetName() and not board_specific_param_set) or last_param_name != param.GetName(): + xml_param = ET.SubElement(xml_group, "airframe") + xml_param.attrib["name"] = param.GetName() + xml_param.attrib["id"] = param.GetId() + xml_param.attrib["maintainer"] = param.GetMaintainer() + last_param_name = param.GetName() + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + if code == "board": + if value == board: + board_specific_param_set = True + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + else: + xml_group.remove(xml_param) + else: + xml_field = ET.SubElement(xml_param, code) + xml_field.text = value + for code in param.GetOutputCodes(): + value = param.GetOutputValue(code) + xml_field = ET.SubElement(xml_param, "output") + xml_field.attrib["name"] = code + xml_field.text = value + if last_param_name != param.GetName(): + board_specific_param_set = False + indent(xml_parameters) + self.xml_document = ET.ElementTree(xml_parameters) + + def Save(self, filename): + self.xml_document.write(filename, encoding="UTF-8") diff --git a/Tools/px_process_airframes.py b/Tools/px_process_airframes.py new file mode 100755 index 0000000000..7d7d28a2b2 --- /dev/null +++ b/Tools/px_process_airframes.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 airframe config processor (main executable file) +# +# This tool scans the PX4 source code for declarations of airframes +# +# Currently supported formats are: +# * XML for the parametric UI generator +# +# + +from __future__ import print_function +import sys +import os +import argparse +from px4airframes import srcscanner, srcparser, xmlout, rcout + +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser(description="Process airframe documentation.") + parser.add_argument("-a", "--airframes-path", + default="../ROMFS/px4fmu_common", + metavar="PATH", + help="path to source files to scan for parameters") + parser.add_argument("-x", "--xml", + nargs='?', + const="airframes.xml", + metavar="FILENAME", + help="Create XML file" + " (default FILENAME: airframes.xml)") + parser.add_argument("-s", "--start-script", + nargs='?', + const="rc.autostart", + metavar="FILENAME", + help="Create start script file") + parser.add_argument("-b", "--board", + nargs='?', + const="", + metavar="BOARD", + help="Board to create airframes xml for") + args = parser.parse_args() + + # Check for valid command + if not (args.xml) and not (args.start_script): + print("Error: You need to specify at least one output method!\n") + parser.print_usage() + sys.exit(1) + + # Initialize source scanner and parser + scanner = srcscanner.SourceScanner() + parser = srcparser.SourceParser() + + # Scan directories, and parse the files + print("Scanning source path " + args.airframes_path) + if not scanner.ScanDir(args.airframes_path, parser): + sys.exit(1) + # We can't validate yet + # if not parser.Validate(): + # sys.exit(1) + param_groups = parser.GetParamGroups() + + # Output to XML file + if args.xml: + print("Creating XML file " + args.xml) + out = xmlout.XMLOutput(param_groups, args.board) + out.Save(args.xml) + + if args.start_script: + print("Creating start script " + args.start_script) + out = rcout.RCOutput(param_groups, args.board) + out.Save(args.start_script) + + print("All done!") + + +if __name__ == "__main__": + main() diff --git a/makefiles/nuttx/nuttx_px4.mk b/makefiles/nuttx/nuttx_px4.mk index f1a88e101e..b5702720d0 100644 --- a/makefiles/nuttx/nuttx_px4.mk +++ b/makefiles/nuttx/nuttx_px4.mk @@ -35,8 +35,8 @@ PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf -PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml -PRODUCT_AIRFRAMEXML = $(WORK_DIR)/airframes.xml +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml +PRODUCT_AIRFRAMEXML = $(WORK_DIR)/airframes.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -49,7 +49,7 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ ifdef GEN_PARAM_XML $(Q) $(PYTHON) $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml - $(Q) $(COPY) $(PX4_BASE)/Tools/airframes.xml $(WORK_DIR)/airframes.xml + $(Q) $(PYTHON) $(PX4_BASE)/Tools/px_process_airframes.py -a $(PX4_BASE)/ROMFS/px4fmu_common/init.d --board CONFIG_ARCH_BOARD_$(CONFIG_BOARD) --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --parameter_xml $(PRODUCT_PARAMXML) \ diff --git a/makefiles/nuttx/nuttx_romfs.mk b/makefiles/nuttx/nuttx_romfs.mk index 611ac18842..5692bd31a7 100644 --- a/makefiles/nuttx/nuttx_romfs.mk +++ b/makefiles/nuttx/nuttx_romfs.mk @@ -68,6 +68,9 @@ ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) LINK_DEPS += $(ROMFS_OBJ) +# Add autostart script +ROMFS_AUTOSTART = $(PX4_BASE)/Tools/px_process_airframes.py + # Remove all comments from startup and mixer files ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py @@ -90,6 +93,10 @@ ifneq ($(ROMFS_EXTRA_FILES),) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras endif + $(Q) $(PYTHON) -u $(ROMFS_AUTOSTART) -a $(ROMFS_ROOT)/init.d -s $(ROMFS_SCRATCH)/init.d/rc.autostart + # Execute in standard dir as well + # so developers notice the generated file + $(Q) $(PYTHON) -u $(ROMFS_AUTOSTART) -a $(ROMFS_ROOT)/init.d -s $(ROMFS_ROOT)/init.d/rc.autostart $(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH) EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index c8098724e8..f307e366ae 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,6 +1,7 @@ uint64 timestamp # Microseconds since system boot bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not armed bool ready_to_arm # Set to true if system is ready to be armed bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/posix-configs/SITL/init/rc.fixed_wing b/posix-configs/SITL/init/rc.fixed_wing index cf8d1017c0..a4a340cc38 100644 --- a/posix-configs/SITL/init/rc.fixed_wing +++ b/posix-configs/SITL/init/rc.fixed_wing @@ -43,3 +43,4 @@ mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 50 -s ATTITUDE -u 14556 mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556 +mavlink boot_complete diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index a1a73f8942..f9518bfa02 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -52,3 +52,4 @@ mavlink stream -r 80 -s ATTITUDE -u 14556 mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink boot_complete diff --git a/posix-configs/SITL/init/rc_iris_ros b/posix-configs/SITL/init/rc_iris_ros index 55710caaa7..9dc28a8505 100644 --- a/posix-configs/SITL/init/rc_iris_ros +++ b/posix-configs/SITL/init/rc_iris_ros @@ -66,4 +66,4 @@ mavlink stream -r 80 -s ATTITUDE -u 14556 mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 - +mavlink boot_complete diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f704df3f9a..79199493fc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -165,7 +165,7 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; - static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -733,7 +733,7 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; if (_servo_armed != set_armed) { _servo_armed = set_armed; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d13673ec9b..53380d75da 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -27,7 +27,8 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) +void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, + const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -45,12 +46,24 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), // accel_earth(0), accel_earth(1), accel_earth(2)); - if (DT > 1.0f) { + bool reset_altitude = false; + + _in_air = in_air; + + if (_update_50hz_last_usec == 0 || DT > DT_MAX) { + DT = 0.02f; // when first starting TECS, use a + // small time constant + reset_altitude = true; + } + + if (!altitude_lock || !in_air) { + reset_altitude = true; + } + + if (reset_altitude) { _integ3_state = baro_altitude; _integ2_state = 0.0f; _integ1_state = 0.0f; - DT = 0.02f; // when first starting TECS, use a - // small time constant } _update_50hz_last_usec = now; @@ -70,7 +83,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // If more than 1 second has elapsed since last update then reset the integrator state // to the measured height - if (DT > 1.0f) { + if (reset_altitude) { _integ3_state = baro_altitude; } else { @@ -95,6 +108,8 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 _vel_dot = 0.0f; } + _states_initalized = true; + } void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, @@ -103,7 +118,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; - _update_speed_last_usec = now; // Convert equivalent airspeeds to true airspeeds @@ -113,7 +127,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _TASmin = indicated_airspeed_min * EAS2TAS; // Reset states of time since last update is too large - if (DT > 1.0f) { + if (_update_speed_last_usec == 0 || DT > 1.0f || !_in_air) { _integ5_state = (_EAS * EAS2TAS); _integ4_state = 0.0f; DT = 0.1f; // when first starting TECS, use a @@ -146,7 +160,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _integ5_state = _integ5_state + integ5_input * DT; // limit the airspeed to a minimum of 3 m/s _integ5_state = max(_integ5_state, 3.0f); - + _update_speed_last_usec = now; } void TECS::_update_speed_demand(void) @@ -471,7 +485,7 @@ void TECS::_update_pitch(void) void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) { // Initialise states and variables if DT > 1 second or in climbout - if (_DT > 1.0f) { + if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air) { _integ6_state = 0.0f; _integ7_state = 0.0f; _last_throttle_dem = throttle_cruise; @@ -484,7 +498,7 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt _TAS_dem_adj = _TAS_dem; _underspeed = false; _badDescent = false; - _DT = 0.1f; // when first starting TECS, use a + _DT = DT_MIN; // when first starting TECS, use a // small time constant } else if (_climbOutDem) { @@ -512,10 +526,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { + if (!_states_initalized) { + return; + } + // Calculate time in seconds since last update uint64_t now = ecl_absolute_time(); _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; - _update_pitch_throttle_last_usec = now; // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); @@ -583,4 +600,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.ptch = _pitch_dem; _tecs_state.dspd_dem = _TAS_rate_dem; + _update_pitch_throttle_last_usec = now; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index eb45237b7d..81a5e2d691 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -80,6 +80,8 @@ public: _SPEdot(0.0f), _SKEdot(0.0f), _airspeed_enabled(false), + _states_initalized(false), + _in_air(false), _throttle_slewrate(0.0f) { } @@ -95,7 +97,8 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); + void update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, + const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air); // Update the control loop calculations void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, @@ -376,7 +379,12 @@ private: // Time since last update of main TECS loop (seconds) float _DT; + static constexpr float DT_MIN = 0.1f; + static constexpr float DT_MAX = 1.0f; + bool _airspeed_enabled; + bool _states_initalized; + bool _in_air; float _throttle_slewrate; float _indicated_airspeed_min; float _indicated_airspeed_max; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 87d9469a1d..6a38d54cb6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -561,6 +561,7 @@ int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; const unsigned cal_hz = 100; const unsigned settle_time = 30; + bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); @@ -599,7 +600,15 @@ int do_level_calibration(int mavlink_fd) { start = hrt_absolute_time(); // average attitude for 5 seconds while(hrt_elapsed_time(&start) < cal_time * 1000000) { - px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + if (pollret <= 0) { + // attitude estimator is not running + mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot"); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + goto out; + } + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); roll_mean += att.roll; pitch_mean += att.pitch; @@ -608,7 +617,6 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - bool success = false; if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index f2f4f0c64f..eb2bcd650e 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -75,12 +75,12 @@ int do_airspeed_calibration(int mavlink_fd) { int result = OK; unsigned calibration_counter = 0; - const unsigned maxcount = 3000; + const unsigned maxcount = 2400; /* give directions */ mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = 2000; + const unsigned calibration_count = (maxcount * 2) / 3; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -204,10 +204,10 @@ int do_airspeed_calibration(int mavlink_fd) /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { - goto error_return; - } - + if (calibrate_cancel_check(mavlink_fd, cancel_sub)) { + goto error_return; + } + /* wait blocking for new data */ px4_pollfd_struct_t fds[1]; fds[0].fd = diff_pres_sub; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index fedafac6cd..4e4ffd3c81 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -470,8 +470,8 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient)); - mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side"); + mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient)); + mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side"); continue; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a29d185e25..88191c5975 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2151,6 +2151,19 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed.timestamp = now; + + /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ + if (safety.safety_switch_available) { + + /* safety is off, go into prearmed */ + armed.prearmed = safety.safety_off; + } else { + /* safety is not present, go into prearmed + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ + armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000); + } orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index c5dab56151..4511882c7b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -67,9 +67,11 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; -static const unsigned max_mags = 3; +static constexpr unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static const unsigned int calibration_sides = 6; +static constexpr unsigned int calibration_sides = 6; ///< The total number of sides +static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer +static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -207,6 +209,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl return false; } +static unsigned progress_percentage(mag_worker_data_t* worker_data) { + return 100 * ((float)worker_data->done_count) / calibration_sides; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -226,7 +232,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta * for a good result, so we're not constraining the user more than we have to. */ - hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 2; + hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; hrt_abstime last_gyro = 0; float gyro_x_integral = 0.0f; float gyro_y_integral = 0.0f; @@ -347,8 +353,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta // Progress indicator for side mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side calibration: progress <%u>", - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + detect_orientation_str(orientation), progress_percentage(worker_data) + + (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); } } else { poll_errcount++; @@ -365,7 +371,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation)); worker_data->done_count++; - mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data)); } return result; @@ -379,8 +385,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_points_perside = 40; - worker_data.calibration_interval_perside_seconds = 20; + worker_data.calibration_points_perside = calibration_total_points / calibration_sides; + worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; // Collect: Right-side up, Left Side, Nose down @@ -499,6 +505,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("RAW DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + printf("RAW: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { @@ -514,6 +524,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag printf("CALIBRATED DATA:\n--------------------\n"); for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + if (worker_data.calibration_counter_total[cur_mag] == 0) { + continue; + } + printf("Calibrated: MAG %u with %u samples:\n", (unsigned)cur_mag, (unsigned)worker_data.calibration_counter_total[cur_mag]); for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 78b68be7db..a155899ef4 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -84,11 +84,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 50.0 + * @min 0.005 + * @max 0.5 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.01f); /** * Maximum positive / up pitch rate. @@ -157,11 +157,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * This gain defines how much control response will result out of a steady * state error. It trims any constant error. * - * @min 0.0 - * @max 100.0 + * @min 0.005 + * @max 0.2 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f); /** * Roll Integrator Anti-Windup diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d1dcdcc0e1..e191d6c75c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1037,7 +1037,7 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - float dt = FLT_MIN; // Using non zero value to a avoid division by zero + float dt = 0.01; // Using non zero value to a avoid division by zero if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } @@ -1045,19 +1045,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; - calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); - float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { - _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + /* tell TECS to update its state, but let it know when it cannot actually control the plane */ + bool in_air_alt_control = (!_vehicle_status.condition_landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); + + /* update TECS filters */ + if (!_mTecs.getEnabled()) { + _tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb, + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); + /* define altitude error */ float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index fc473ebabd..dac5e6a3f9 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), - _airspeed({}), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize() // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); updateParameterCache(true); } @@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions() { orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() @@ -81,6 +89,11 @@ bool FixedwingLandDetector::update() // First poll for new data from our subscriptions updateSubscriptions(); + // only trigger flight conditions if we are armed + if (!_arming.armed) { + return true; + } + const uint64_t now = hrt_absolute_time(); bool landDetected = false; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 0e9c092ee0..325faee794 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,7 +44,9 @@ #include "LandDetector.h" #include #include +#include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -90,11 +92,15 @@ private: } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - int _parameterSub; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + int _vehicleStatusSub; + int _armingSub; + struct airspeed_s _airspeed; + struct vehicle_status_s _vehicleStatus; + struct actuator_armed_s _arming; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index e4494ad56b..7f719d4cef 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -46,8 +46,9 @@ LandDetector::LandDetector() : _landDetectedPub(0), _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false) { // ctor } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index f5ca4f3265..2ade4ac8c0 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -96,10 +96,12 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: - uintptr_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + uintptr_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + uint64_t _arming_time; /**< timestamp of arming time */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5391f7769a..1d0a6b92dc 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _attitudeSub(-1), - _vehicleGlobalPosition({}), - _vehicleStatus({}), - _actuators({}), - _arming({}), - _vehicleAttitude({}), + _attitudeSub(-1), + _vehicleGlobalPosition{}, + _vehicleStatus{}, + _actuators{}, + _arming{}, + _vehicleAttitude{}, _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); @@ -97,7 +97,10 @@ bool MulticopterLandDetector::update() // only trigger flight conditions if we are armed if (!_arming.armed) { + _arming_time = 0; return true; + } else if (_arming_time == 0) { + _arming_time = hrt_absolute_time(); } // return status based on armed state if no position lock is available @@ -110,8 +113,18 @@ bool MulticopterLandDetector::update() const uint64_t now = hrt_absolute_time(); - // check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; + float armThresholdFactor = 1.0f; + + // Widen acceptance thresholds for landed state right after arming + // so that motor spool-up and other effects do not trigger false negatives + if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) { + armThresholdFactor = 2.5f; + } + + // check if we are moving vertically - this might see a spike after arming due to + // throttle-up vibration. If accelerating fast the throttle thresholds will still give + // an accurate in-air indication + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor; // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n @@ -119,9 +132,11 @@ bool MulticopterLandDetector::update() && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving - bool rotating = fabsf(_vehicleAttitude.rollspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.yawspeed) > _params.maxRotation; + float maxRotationScaled = _params.maxRotation * armThresholdFactor; + + bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 8c57416b56..d001be4e7f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -97,20 +97,20 @@ private: } _params; private: - int _vehicleGlobalPositionSub; /**< notification of global position */ + int _vehicleGlobalPositionSub; /**< notification of global position */ int _vehicleStatusSub; int _actuatorsSub; int _armingSub; int _parameterSub; int _attitudeSub; - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct vehicle_status_s _vehicleStatus; - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - struct vehicle_attitude_s _vehicleAttitude; + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct vehicle_status_s _vehicleStatus; + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + struct vehicle_attitude_s _vehicleAttitude; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 77bac2ad74..57e616169b 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -49,7 +49,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.70f); /** * Multicopter max horizontal velocity diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 55fde3bfed..6b61c7927f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -120,6 +120,8 @@ extern mavlink_system_t mavlink_system; static void usage(void); +bool Mavlink::_boot_complete = false; + Mavlink::Mavlink() : #ifndef __PX4_NUTTX VDev("mavlink-log", MAVLINK_LOG_DEVICE), @@ -1639,15 +1641,15 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("HIGHRES_IMU", 2.0f); + configure_stream("ATTITUDE", 20.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("RC_CHANNELS", 1.0f); + configure_stream("RC_CHANNELS", 4.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("ATTITUDE_TARGET", 8.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; @@ -2163,6 +2165,10 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); + } else if (!strcmp(argv[1], "boot_complete")) { + Mavlink::set_boot_complete(); + return 0; + } else { usage(); return 1; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index dd6b2ac749..7b74fb8f09 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -163,6 +163,14 @@ public: bool get_forwarding_on() { return _forwarding_on; } + /** + * Set the boot complete flag on all instances + * + * Setting the flag unblocks parameter transmissions, which are gated + * beforehand to ensure that the system is fully initialized. + */ + static void set_boot_complete() { _boot_complete = true; } + /** * Get the free space in the transmit buffer * @@ -325,6 +333,8 @@ public: int get_socket_fd () { return _socket_fd; }; + static bool boot_complete() { return _boot_complete; } + protected: Mavlink *next; @@ -333,6 +343,7 @@ private: int _mavlink_fd; bool _task_running; + static bool _boot_complete; /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 9ade6fbc56..1b7f1d95dd 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -193,7 +193,7 @@ void MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && t > 4 * 1000 * 1000) { + if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3c79d4f4d2..f068a25189 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -35,9 +35,9 @@ * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * - * @author Lorenz Meier - * @author Anton Babushkin - * @author Thomas Gubler + * @author Lorenz Meier + * @author Anton Babushkin + * @author Thomas Gubler */ /* XXX trim includes */ @@ -136,7 +136,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0), - _orb_class_instance(-1) + _orb_class_instance(-1), + _mom_switch_pos{}, + _mom_switch_state(0) { } @@ -970,15 +972,45 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } } -static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) { +switch_pos_t +MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) +{ + // XXX non-standard 3 pos switch decoding return (buttons >> (sw * 2)) & 3; } -static int decode_switch_pos_n(uint16_t buttons, int sw) { - if (buttons & (1 << sw)) { - return 1; +int +MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) +{ + + bool on = (buttons & (1 << sw)); + + if (sw < MOM_SWITCH_COUNT) { + + bool last_on = (_mom_switch_state & (1 << sw)); + + /* first switch is 2-pos, rest is 2 pos */ + unsigned state_count = (sw == 0) ? 3 : 2; + + /* only transition on low state */ + if (!on && (on != last_on)) { + + _mom_switch_pos[sw]++; + if (_mom_switch_pos[sw] == state_count) { + _mom_switch_pos[sw] = 0; + } + } + + /* state_count - 1 is the number of intervals and 1000 is the range, + * with 2 states 0 becomes 0, 1 becomes 1000. With + * 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000, + * and so on for more states. + */ + return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000; + } else { - return 0; + /* return the current state */ + return on * 1000 + 1000; } } @@ -1075,12 +1107,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.values[3] = 1000; } - rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000; - rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000; - rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000; - rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000; - rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000; - rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000; + /* decode all switches which fit into the channel mask */ + unsigned max_switch = (sizeof(man.buttons) * 8); + unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0])); + if (max_switch > (max_channels - 4)) { + max_switch = (max_channels - 4); + } + + /* fill all channels */ + for (unsigned i = 0; i < max_switch; i++) { + rc.values[i + 4] = decode_switch_pos_n(man.buttons, i); + } + _mom_switch_state = man.buttons; if (_rc_pub == nullptr) { _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b2e7a9d6cb..3f115e8718 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -35,8 +35,8 @@ * @file mavlink_orb_listener.h * MAVLink 1.0 uORB listener definition * - * @author Lorenz Meier - * @author Anton Babushkin + * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -141,15 +141,26 @@ private: void *receive_thread(void *arg); /** - * Convert remote timestamp to local hrt time (usec) - * Use timesync if available, monotonic boot time otherwise - */ + * Convert remote timestamp to local hrt time (usec) + * Use timesync if available, monotonic boot time otherwise + */ uint64_t sync_stamp(uint64_t usec); + /** - * Exponential moving average filter to smooth time offset - */ + * Exponential moving average filter to smooth time offset + */ void smooth_time_offset(uint64_t offset_ns); + /** + * Decode a switch position from a bitfield + */ + switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw); + + /** + * Decode a switch position from a bitfield and state + */ + int decode_switch_pos_n(uint16_t buttons, unsigned sw); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_land_detected_s hil_land_detector; @@ -195,6 +206,11 @@ private: uint64_t _time_offset; int _orb_class_instance; + static constexpr unsigned MOM_SWITCH_COUNT = 8; + + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT]; + uint16_t _mom_switch_state; + /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); MavlinkReceiver operator=(const MavlinkReceiver &); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f42fa9fe4e..8d2ebbb6af 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,6 +83,9 @@ #include #include +#include +#include + #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f @@ -95,7 +98,7 @@ */ extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); -class MulticopterPositionControl +class MulticopterPositionControl : public control::SuperBlock { public: /** @@ -122,15 +125,15 @@ private: int _control_task; /**< task handle for task */ int _mavlink_fd; /**< mavlink fd */ - int _vehicle_status_sub; /**< vehicle status subscription */ - int _att_sub; /**< vehicle attitude subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ + int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< position setpoint triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ int _local_pos_sp_sub; /**< offboard local position setpoint */ int _global_vel_sp_sub; /**< offboard global velocity setpoint */ @@ -138,17 +141,19 @@ private: orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ - struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + control::BlockParamFloat _manual_thr_min; + control::BlockParamFloat _manual_thr_max; struct { param_t thr_min; @@ -213,7 +218,7 @@ private: /** * Update our local parameter cache. */ - int parameters_update(bool force); + int parameters_update(bool force); /** * Update control outputs @@ -293,7 +298,7 @@ MulticopterPositionControl *g_control; } MulticopterPositionControl::MulticopterPositionControl() : - + SuperBlock(NULL, "MPC"), _task_should_exit(false), _control_task(-1), _mavlink_fd(-1), @@ -313,7 +318,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), - + _manual_thr_min(this, "MANTHR_MIN"), + _manual_thr_max(this, "MANTHR_MAX"), _ref_alt(0.0f), _ref_timestamp(0), @@ -413,6 +419,10 @@ MulticopterPositionControl::parameters_update(bool force) } if (updated || force) { + /* update C++ param system */ + updateParams(); + + /* update legacy C interface params */ param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); @@ -1416,11 +1426,11 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = math::min(_manual.z, _params.thr_max); + _att_sp.thrust = math::min(_manual.z, _manual_thr_max.get()); /* enforce minimum throttle if not landed */ if (!_vehicle_status.condition_landed) { - _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get()); } } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index a3793b0cc0..d29d11c65d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -41,7 +41,32 @@ #include /** - * Minimum thrust + * Minimum thrust in auto thrust control + * + * It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.05 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Maximum thrust in auto thrust control + * + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. + * + * @min 0.0 + * @max 0.95 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); + +/** + * Minimum manual thrust * * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. * @@ -49,10 +74,10 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f); /** - * Maximum thrust + * Maximum manual thrust * * Limit max allowed thrust. Setting a value of one can put * the system into actuator saturation as no spread between @@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f); /** * Proportional gain for vertical position error diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 4d78d245d9..10a07323c6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -157,7 +157,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1); * @max 1 * @group SD Logging */ -PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0); +PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1); #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index ef097e648f..dd297a95ee 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -680,7 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user * to fine tune the board offset in the event of misalignment. * - * @unit radians + * @unit degrees * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); @@ -691,7 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user * to fine tune the board offset in the event of misalignment. * - * @unit radians + * @unit degrees * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); @@ -702,7 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user * to fine tune the board offset in the event of misalignment. * - * @unit radians + * @unit degrees * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); @@ -2273,7 +2273,7 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE * THE SYSTEM TO PUT CHANGES INTO EFFECT. * - * Set to 1000 for default or 900 to increase servo travel + * Set to 1000 for industry default or 900 to increase servo travel. * * @min 800 * @max 1400 @@ -2289,7 +2289,7 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000); * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE * THE SYSTEM TO PUT CHANGES INTO EFFECT. * - * Set to 2000 for default or 2100 to increase servo travel + * Set to 2000 for industry default or 2100 to increase servo travel. * * @min 1600 * @max 2200