From 255d91d8d49ce06f065b6a0269bdfabeaa40fae4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 7 Jan 2014 21:56:35 +0100 Subject: hw_ver app added for hardware version checking --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +++++-- ROMFS/px4fmu_common/init.d/800_sdlogger | 4 +++- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 4 +++- ROMFS/px4fmu_common/init.d/rc.sensors | 4 +--- ROMFS/px4fmu_common/init.d/rcS | 9 ++++++--- 6 files changed, 19 insertions(+), 11 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 9b664d63e..25ea25ae8 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -67,8 +67,11 @@ if px4io start then echo "IO started" else - fmu mode_serial - echo "FMU started" + if hw_ver compare PX4FMU_V1 + then + fmu mode_serial + echo "FMU started" + fi fi # diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger index 9b90cbdd0..2d2c3737b 100644 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ b/ROMFS/px4fmu_common/init.d/800_sdlogger @@ -42,10 +42,12 @@ position_estimator_inav start if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -e -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -e -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b316..24784610c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ then # # Disable px4io topic limiting # - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then px4io limit 200 else diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..1791acbee 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,10 +5,12 @@ if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then + echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 16 else + echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e3..a2517135f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -28,9 +28,7 @@ fi if lsm303d start then - set BOARD fmuv2 -else - set BOARD fmuv1 + echo "using LSM303D" fi # Start airspeed sensors diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 66cb3f237..8801d1126 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -185,9 +185,12 @@ then else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + sleep 10 + reboot + fi fi else echo "PX4IO update failed" -- cgit v1.2.3 From 4cffd99db940a9f0cda7643842ccf17d8a3f1b48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 20:55:12 +0100 Subject: Major autostart rewrite --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 138 +++------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 67 ++--- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 66 +---- ROMFS/px4fmu_common/init.d/rc.autostart | 180 +++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 +++ ROMFS/px4fmu_common/init.d/rc.io | 32 +-- ROMFS/px4fmu_common/init.d/rc.mc_apps | 24 ++ ROMFS/px4fmu_common/init.d/rc.mc_interface | 23 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 39 --- ROMFS/px4fmu_common/init.d/rcS | 383 ++++++++++------------------ 10 files changed, 477 insertions(+), 509 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 25ea25ae8..8732673f7 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -1,108 +1,42 @@ #!nsh -# -# USB HIL start -# -echo "[HIL] HILS quadrotor starting.." +echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - if hw_ver compare PX4FMU_V1 - then - fmu mode_serial - echo "FMU started" - fi + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e2..58c6d99bb 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,30 +2,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 + # + # Default parameters for this platform + # param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 @@ -38,32 +35,12 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 - - param save fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191..ca055dfcb 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,14 +2,11 @@ echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - + # + # Default parameters for this platform + # param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_P 0.12 @@ -22,53 +19,12 @@ then param set MC_YAWRATE_D 0.005 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 +set PWM_MIN 1200 +set PWM_MAX 1900 +set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 000000000..10b7bc424 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,180 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# 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 + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +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/666_fmu_q_x550 +fi + +if param compare SYS_AUTOSTART 6012 12 +then + #set MIXER /etc/mixers/FMU_hex_x.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 7013 13 +then + #set MIXER /etc/mixers/FMU_hex_+.mix + #sh /etc/init.d/rc.hexa +fi + +if param compare SYS_AUTOSTART 8001 +then + #set MIXER /etc/mixers/FMU_octo_x.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 9001 +then + #set MIXER /etc/mixers/FMU_octo_+.mix + #sh /etc/init.d/rc.octo +fi + +if param compare SYS_AUTOSTART 12001 +then + #set MIXER /etc/mixers/FMU_octo_cox.mix + #sh /etc/init.d/rc.octo +fi + +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 4017 17 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5018 18 +then + #set MKBLCTRL_MODE no + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4019 19 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME x + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 5020 20 +then + #set MKBLCTRL_MODE yes + #set MKBLCTRL_FRAME + + #sh /etc/init.d/rc.custom_dji_f330_mkblctrl +fi + +if param compare SYS_AUTOSTART 4021 21 +then + #set FRAME_GEOMETRY x + #set ESC_MAKER afro + #sh /etc/init.d/rc.custom_io_esc +fi + +if param compare SYS_AUTOSTART 10022 22 +then + #set FRAME_GEOMETRY w + #sh /etc/init.d/rc.custom_io_esc +fi + +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_io_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_io_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + #sh /etc/init.d/3034_io_fx79 + #set MODE custom +fi + +if param compare SYS_AUTOSTART 40 +then + #sh /etc/init.d/40_io_segway + #set MODE custom +fi + +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_hk_bixler + #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 800 +then + #sh /etc/init.d/800_sdlogger + #set MODE custom +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil new file mode 100644 index 000000000..d5fc5eb08 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart_hil @@ -0,0 +1,34 @@ +# +# Check if auto-setup from one of the standard scripts for HIL is wanted +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1002 +then + #sh /etc/init.d/1002_rc_fw_state.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1003 +then + #sh /etc/init.d/1003_rc_quad_+.hil + set MODE hil +fi + +if param compare SYS_AUTOSTART 1004 +then + #sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE hil +fi + diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 24784610c..5a010cc9b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,19 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if hw_ver compare PX4FMU_V1 - then - px4io limit 200 - else - px4io limit 400 - fi +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust px4io topic limiting +# +if hw_ver compare PX4FMU_V1 +then + px4io limit 200 else - # SOS - tone_alarm error + px4io limit 400 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps new file mode 100644 index 000000000..16a7a33c6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, actuator output and mavlink +# + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 6bb2e84ec..2a05012a6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -35,15 +35,26 @@ else fi fi - # # Set PWM output frequency # -pwm rate -c $OUTPUTS -r $PWM_RATE +if [ $PWM_RATE != none ] +then + pwm rate -c $OUTPUTS -r $PWM_RATE +fi # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM values # -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX +if [ $PWM_DISARMED != none ] +then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED +fi +if [ $PWM_MIN != none ] +then + pwm min -c $OUTPUTS -p $PWM_MIN +fi +if [ $PWM_MAX != none ] +then + pwm max -c $OUTPUTS -p $PWM_MAX +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor deleted file mode 100644 index bc550ac5a..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ /dev/null @@ -1,39 +0,0 @@ -#!nsh -# -# Standard everything needed for multirotors except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8801d1126..4dd9af407 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,12 +60,9 @@ fi if [ $MODE == autostart ] then # - # Start terminal + # Start CDC/ACM serial driver # - if sercon - then - echo "USB connected" - fi + sercon # # Start the ORB (first app to start) @@ -107,52 +104,79 @@ then blinkm systemstate fi fi + + set USE_IO no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi # # Start the Commander (needs to be this early for in-air-restarts) # commander start - - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi + # + # Set parameters and env variables for selected AUTOSTART (HIL setups) + # + sh /etc/init.d/rc.autostart_hil - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi + if [ $MODE == hil ] + then + # + # Do common HIL setup depending on env variables + # + # Allow USB some time to come up + sleep 1 + + # Start MAVLink on USB port + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + + # Create a fake HIL /dev/pwm_output interface + hil mode_pwm + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - if [ $MODE != custom ] - then - # Try to get an USB console + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi + else + # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & fi - + # # Upgrade PX4IO firmware # - if [ -f /etc/extras/px4io-v2_default.bin ] then set io_file /etc/extras/px4io-v2_default.bin @@ -170,6 +194,7 @@ then then echo "PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile + set USE_IO yes else echo "PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -182,6 +207,7 @@ then echo "PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA + set USE_IO yes else echo "PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -200,222 +226,91 @@ then fi set EXIT_ON_END no + set USE_LOGGING yes + set USE_GPS yes # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) + # Set parameters and env variables for selected AUTOSTART # - # 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 - - if param compare SYS_AUTOSTART 4008 8 - then - sh /etc/init.d/4008_ardrone - set MODE custom - fi - - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi - - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi + sh /etc/init.d/rc.autostart - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] then - sh /etc/init.d/3031_io_phantom - set MODE custom + param set SYS_AUTOCONFIG 0 + param save fi - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi + if [ $MODE == autostart ] + then + # + # Do common setup depending on env variables + # + if [ $USE_IO == yes ] + then + echo "Use IO" + + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + else + echo "Don't use IO" + + # Start MAVLink on ttyS0 + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Configure FMU for PWM outputs + fmu mode_pwm + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + fi + + # Sensors + echo "Start sensors" + sh /etc/init.d/rc.sensors + + # Logging + if [ $USE_LOGGING == yes ] + then + sh /etc/init.d/rc.logging + fi + + # GPS interface + if [ $USE_GPS == yes ] + then + gps start + fi + + # + # Fixed wing setup + # + if [ $FRAME_TYPE == fw ] + then + echo "Setup FIXED WING" + fi - 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_hk_bixler - set MODE custom + # + # Multicopters setup + # + if [ $FRAME_TYPE == mc ] + then + echo "Setup MULTICOPTER" + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start common multicopter apps + sh /etc/init.d/rc.mc_apps + fi 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 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing + # Start any custom extensions if [ -f /fs/microsd/etc/rc.local ] then sh /fs/microsd/etc/rc.local -- cgit v1.2.3 From 9a5ef700709b50d57201e77bc80f11c47b25f548 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 23:31:49 +0100 Subject: init: USE_LOGGING and USE_GPS env vars removed --- ROMFS/px4fmu_common/init.d/rcS | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4dd9af407..7d38736de 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -226,8 +226,6 @@ then fi set EXIT_ON_END no - set USE_LOGGING yes - set USE_GPS yes # # Set parameters and env variables for selected AUTOSTART @@ -276,16 +274,10 @@ then sh /etc/init.d/rc.sensors # Logging - if [ $USE_LOGGING == yes ] - then - sh /etc/init.d/rc.logging - fi + sh /etc/init.d/rc.logging # GPS interface - if [ $USE_GPS == yes ] - then - gps start - fi + gps start # # Fixed wing setup -- cgit v1.2.3 From 532c4c771e3da9d0b371101a056c29d0f417cd09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 Jan 2014 10:05:24 +0100 Subject: Autostart: generic quad, hexa and octo added, WIP --- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 26 +++++---- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 32 +++++----- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 27 +++++++++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 ------------------------ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 ++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 31 +++++----- ROMFS/px4fmu_common/init.d/rc.hexa | 94 ------------------------------ ROMFS/px4fmu_common/init.d/rc.mc_interface | 42 ++++++------- 13 files changed, 196 insertions(+), 234 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm new file mode 100644 index 000000000..9bee414dc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 58c6d99bb..93edb0005 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,24 +1,29 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.8 param set MC_YAWPOS_I 0.0 param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 param set MPC_TILT_MAX 0.5 param set MPC_THR_MAX 0.8 @@ -37,10 +42,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ca055dfcb..cc0ddccc8 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,30 +1,34 @@ #!nsh +# Maintainers: Lorenz Meier + echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 set PWM_MIN 1200 set PWM_MAX 1900 -set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 000000000..a749b7699 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,27 @@ +#!nsh + +# Maintainers: Todd Stellanova + +echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" + +# Use generic quad X PWM as base +sh /etc/init.d/4001_quad_x_pwm + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWPOS_P 0.6 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_D 0 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 +fi diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..7d0748668 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..3ea5479cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 6 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb..000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..f747618b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 4 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f8f459185 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY x +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..f8bcd13a9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,17 @@ +#!nsh + +# Maintainers: Anton Babushkin , Lorenz Meier + +echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # +fi + +set FRAME_TYPE mc +set FRAME_GEOMETRY + +set FRAME_COUNT 8 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 10b7bc424..153fbb66b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,53 +17,54 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 +then + sh /etc/init.d/4001_quad_x_pwm +fi + +if param compare SYS_AUTOSTART 4008 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 9 +if param compare SYS_AUTOSTART 4009 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 10 +if param compare SYS_AUTOSTART 4010 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 11 +if param compare SYS_AUTOSTART 4011 then sh /etc/init.d/4011_dji_f450 fi if param compare SYS_AUTOSTART 4012 then - #sh /etc/init.d/666_fmu_q_x550 + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 6012 12 +if param compare SYS_AUTOSTART 6001 then - #set MIXER /etc/mixers/FMU_hex_x.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/6001_hexa_x_pwm fi -if param compare SYS_AUTOSTART 7013 13 +if param compare SYS_AUTOSTART 7001 then - #set MIXER /etc/mixers/FMU_hex_+.mix - #sh /etc/init.d/rc.hexa + sh /etc/init.d/7001_hexa_+_pwm fi if param compare SYS_AUTOSTART 8001 then - #set MIXER /etc/mixers/FMU_octo_x.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/8001_octo_x_pwm fi if param compare SYS_AUTOSTART 9001 then - #set MIXER /etc/mixers/FMU_octo_+.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/9001_octo_+_pwm fi if param compare SYS_AUTOSTART 12001 diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e4..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 2a05012a6..9e3d678bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -3,38 +3,34 @@ # Script to set PWM min / max limits and mixer # -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - +echo "Rotors count: $FRAME_COUNT" if [ $FRAME_COUNT == 4 ] then + set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then +fi +if [ $FRAME_COUNT == 6 ] +then + set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 - else +fi +if [ $FRAME_COUNT == 8 ] +then + set FRAME_COUNT_STR octo set OUTPUTS 12345678 - fi + param set MAV_TYPE 14 fi +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} + # # Set PWM output frequency # -- cgit v1.2.3 From b5d56523bc100d7bf95a6dfbac95c1afc89e345e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 13:18:34 +0100 Subject: Init scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 85 ++---- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 98 +++---- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 7 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 58 +--- ROMFS/px4fmu_common/init.d/4001_quad_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 10 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 12 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 +- ROMFS/px4fmu_common/init.d/40_io_segway | 51 ---- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 17 -- ROMFS/px4fmu_common/init.d/800_sdlogger | 53 ---- ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 17 -- ROMFS/px4fmu_common/init.d/rc.autostart | 154 ++++------- ROMFS/px4fmu_common/init.d/rc.autostart_hil | 34 --- .../init.d/rc.custom_dji_f330_mkblctrl | 113 -------- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 -------- ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 ++ ROMFS/px4fmu_common/init.d/rc.fw_interface | 18 ++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 70 ++--- ROMFS/px4fmu_common/init.d/rc.octo | 94 ------- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 --- ROMFS/px4fmu_common/init.d/rcS | 306 +++++++++++---------- 28 files changed, 372 insertions(+), 1122 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart_hil delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 81d4b5d57..610d482c1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,31 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] Team Blacksheep Discovery Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set FRAME_TYPE mc +set FRAME_GEOMETRY quad_w +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1100 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79..de5028052 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,43 @@ #!nsh +# Maintainers: Anton Babushkin + echo "[init] 3DR Iris Quad" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 +# Use generic wide arm quad X PWM as base +sh /etc/init.d/10001_quad_w - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 8732673f7..7d21176f2 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -37,6 +37,7 @@ then param set MPC_Z_VEL_P 0.20 fi -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c90..9bf01c60c 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,12 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" +echo "[init] EasyStar" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +30,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set FRAME_TYPE fw +set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm b/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm deleted file mode 100644 index 9bee414dc..000000000 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 93edb0005..0cfe68b27 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,10 +2,7 @@ # Maintainers: Anton Babushkin -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F330" if [ $DO_AUTOCONFIG == yes ] then @@ -42,6 +39,11 @@ then param set MPC_Z_VEL_P 0.20 fi +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cc0ddccc8..4a0953a2e 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,10 +2,7 @@ # Maintainers: Lorenz Meier -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm +echo "[init] DJI F450" if [ $DO_AUTOCONFIG == yes ] then @@ -27,7 +24,12 @@ then # TODO add default MPC parameters fi - + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 + # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index a749b7699..396349d9c 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -4,9 +4,6 @@ echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" -# Use generic quad X PWM as base -sh /etc/init.d/4001_quad_x_pwm - if [ $DO_AUTOCONFIG == yes ] then # @@ -25,3 +22,8 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_x + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b440..000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 7d0748668..000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic quadcopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index 3ea5479cb..000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 6 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index f747618b8..000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic hexacopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 4 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f8f459185..000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter X with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY x -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 2d2c3737b..000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,53 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param save -fi - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if hw_ver compare PX4FMU_V1 - then - echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -e -b 16 - else - echo "Start sdlog2 at 200Hz" - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index f8bcd13a9..000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,17 +0,0 @@ -#!nsh - -# Maintainers: Anton Babushkin , Lorenz Meier - -echo "[init] PX4FMU v1, v2 with or without IO on generic octocopter + with PWM ESCs" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # -fi - -set FRAME_TYPE mc -set FRAME_GEOMETRY + -set FRAME_COUNT 8 -set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 153fbb66b..9da0135b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -17,114 +17,62 @@ # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox -if param compare SYS_AUTOSTART 4001 -then - sh /etc/init.d/4001_quad_x_pwm -fi - -if param compare SYS_AUTOSTART 4008 -then - #sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4009 -then - #sh /etc/init.d/4009_ardrone_flow -fi - -if param compare SYS_AUTOSTART 4010 -then - sh /etc/init.d/4010_dji_f330 -fi - -if param compare SYS_AUTOSTART 4011 -then - sh /etc/init.d/4011_dji_f450 -fi - -if param compare SYS_AUTOSTART 4012 -then - sh /etc/init.d/4012_hk_x550 -fi - -if param compare SYS_AUTOSTART 6001 -then - sh /etc/init.d/6001_hexa_x_pwm -fi - -if param compare SYS_AUTOSTART 7001 -then - sh /etc/init.d/7001_hexa_+_pwm -fi - -if param compare SYS_AUTOSTART 8001 -then - sh /etc/init.d/8001_octo_x_pwm -fi +# +# Simulation setups +# -if param compare SYS_AUTOSTART 9001 +if param compare SYS_AUTOSTART 1000 then - sh /etc/init.d/9001_octo_+_pwm + #sh /etc/init.d/1000_rc_fw_easystar.hil fi -if param compare SYS_AUTOSTART 12001 +if param compare SYS_AUTOSTART 1001 then - #set MIXER /etc/mixers/FMU_octo_cox.mix - #sh /etc/init.d/rc.octo + sh /etc/init.d/1001_rc_quad.hil fi -if param compare SYS_AUTOSTART 10015 15 +if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/10015_tbs_discovery + #sh /etc/init.d/1002_rc_fw_state.hil fi -if param compare SYS_AUTOSTART 10016 16 +if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/10016_3dr_iris + #sh /etc/init.d/1003_rc_quad_+.hil fi -if param compare SYS_AUTOSTART 4017 17 +if param compare SYS_AUTOSTART 1004 then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/1004_rc_fw_Rascal110.hil fi -if param compare SYS_AUTOSTART 5018 18 -then - #set MKBLCTRL_MODE no - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl -fi +# +# Standard plane +# -if param compare SYS_AUTOSTART 4019 19 +if param compare SYS_AUTOSTART 2100 100 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME x - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2100_mpx_easystar + #set MODE custom fi -if param compare SYS_AUTOSTART 5020 20 +if param compare SYS_AUTOSTART 2101 101 then - #set MKBLCTRL_MODE yes - #set MKBLCTRL_FRAME + - #sh /etc/init.d/rc.custom_dji_f330_mkblctrl + #sh /etc/init.d/2101_hk_bixler + #set MODE custom fi -if param compare SYS_AUTOSTART 4021 21 +if param compare SYS_AUTOSTART 2102 102 then - #set FRAME_GEOMETRY x - #set ESC_MAKER afro - #sh /etc/init.d/rc.custom_io_esc + #sh /etc/init.d/2102_3dr_skywalker + #set MODE custom fi -if param compare SYS_AUTOSTART 10022 22 -then - #set FRAME_GEOMETRY w - #sh /etc/init.d/rc.custom_io_esc -fi +# +# Flying wing +# -if param compare SYS_AUTOSTART 3030 30 +if param compare SYS_AUTOSTART 3030 then #sh /etc/init.d/3030_io_camflyer fi @@ -147,35 +95,47 @@ fi if param compare SYS_AUTOSTART 3034 34 then #sh /etc/init.d/3034_io_fx79 - #set MODE custom fi -if param compare SYS_AUTOSTART 40 +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 then - #sh /etc/init.d/40_io_segway - #set MODE custom + #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 2100 100 +if param compare SYS_AUTOSTART 4009 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 2101 101 +if param compare SYS_AUTOSTART 4010 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 2102 102 +if param compare SYS_AUTOSTART 4011 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 +then + sh /etc/init.d/4012_hk_x550 fi -if param compare SYS_AUTOSTART 800 +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 then - #sh /etc/init.d/800_sdlogger - #set MODE custom + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 +then + sh /etc/init.d/10016_3dr_iris fi diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_hil b/ROMFS/px4fmu_common/init.d/rc.autostart_hil deleted file mode 100644 index d5fc5eb08..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.autostart_hil +++ /dev/null @@ -1,34 +0,0 @@ -# -# Check if auto-setup from one of the standard scripts for HIL is wanted -# - -if param compare SYS_AUTOSTART 1000 -then - #sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1001 -then - sh /etc/init.d/1001_rc_quad.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1002 -then - #sh /etc/init.d/1002_rc_fw_state.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1003 -then - #sh /etc/init.d/1003_rc_quad_+.hil - set MODE hil -fi - -if param compare SYS_AUTOSTART 1004 -then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE hil -fi - diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e52..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f02851006..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 000000000..d354fb06f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface new file mode 100644 index 000000000..f9517f422 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -0,0 +1,18 @@ +#!nsh +# +# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +# +# Load mixer +# +echo "Frame geometry: ${FRAME_GEOMETRY}" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: ${MIXER}" +mixer load /dev/pwm_output ${MIXER} diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 16a7a33c6..8b51d57e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, actuator output and mavlink +# Standard apps for multirotors # # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 9e3d678bf..003aee81b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,24 +1,25 @@ #!nsh # -# Script to set PWM min / max limits and mixer +# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output # -echo "Rotors count: $FRAME_COUNT" -if [ $FRAME_COUNT == 4 ] +if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] then - set FRAME_COUNT_STR quad set OUTPUTS 1234 param set MAV_TYPE 2 fi -if [ $FRAME_COUNT == 6 ] +if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] +then + set OUTPUTS 1234 + param set MAV_TYPE 2 +fi +if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] then - set FRAME_COUNT_STR hex set OUTPUTS 123456 param set MAV_TYPE 13 fi -if [ $FRAME_COUNT == 8 ] +if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] then - set FRAME_COUNT_STR octo set OUTPUTS 12345678 param set MAV_TYPE 14 fi @@ -26,31 +27,34 @@ fi # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY_STR}_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix +echo "Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER -# -# Set PWM output frequency -# -if [ $PWM_RATE != none ] -then - pwm rate -c $OUTPUTS -r $PWM_RATE -fi - -# -# Set disarmed, min and max PWM values -# -if [ $PWM_DISARMED != none ] -then - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -fi -if [ $PWM_MIN != none ] -then - pwm min -c $OUTPUTS -p $PWM_MIN -fi -if [ $PWM_MAX != none ] +if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then - pwm max -c $OUTPUTS -p $PWM_MAX + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + pwm rate -c $OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + pwm disarmed -c $OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + pwm min -c $OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + pwm max -c $OUTPUTS -p $PWM_MAX + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47e..0cd8a0e04 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7d38736de..09d66cf41 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -52,7 +52,7 @@ then echo "[init] USB interface connected" fi - echo "Running rc.APM" + echo "[init] Running rc.APM" # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi @@ -85,9 +85,9 @@ then then if param load /fs/microsd/params then - echo "Parameters loaded" + echo "[init] Parameters loaded" else - echo "Parameter file corrupt - ignoring" + echo "[init] Parameter file corrupt - ignoring" fi fi #fi @@ -97,7 +97,7 @@ then # if rgbled start then - echo "Using external RGB Led" + echo "[init] Using external RGB Led" else if blinkm start then @@ -105,75 +105,10 @@ then fi fi - set USE_IO no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none - - if param compare SYS_AUTOCONFIG 1 - then - set DO_AUTOCONFIG yes - else - set DO_AUTOCONFIG no - fi - - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - - # - # Set parameters and env variables for selected AUTOSTART (HIL setups) - # - sh /etc/init.d/rc.autostart_hil + # Use FMU PWM output by default + set OUTPUT_MODE fmu_pwm + set IO_PRESENT no - if [ $MODE == hil ] - then - # - # Do common HIL setup depending on env variables - # - # Allow USB some time to come up - sleep 1 - - # Start MAVLink on USB port - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - - # Create a fake HIL /dev/pwm_output interface - hil mode_pwm - - # Sensors - echo "Start sensors" - sh /etc/init.d/rc.sensors - - # - # Fixed wing setup - # - if [ $FRAME_TYPE == fw ] - then - echo "Setup FIXED WING" - fi - - # - # Multicopters setup - # - if [ $FRAME_TYPE == mc ] - then - echo "Setup MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start common multicopter apps - sh /etc/init.d/rc.mc_apps - fi - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - # # Upgrade PX4IO firmware # @@ -184,19 +119,17 @@ then set io_file /etc/extras/px4io-v1_default.bin fi - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi - if px4io checkcrc $io_file then - echo "PX4IO CRC OK" + echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO CRC failure" + echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile tone_alarm MBABGP if px4io forceupdate 14662 $io_file @@ -204,12 +137,16 @@ then usleep 500000 if px4io start then - echo "PX4IO restart OK" + echo "[init] PX4IO restart OK" echo "PX4IO restart OK" >> $logfile tone_alarm MSPAA - set USE_IO yes + + set IO_PRESENT yes + + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm else - echo "PX4IO restart failed" + echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile if hw_ver compare PX4FMU_V2 then @@ -219,18 +156,54 @@ then fi fi else - echo "PX4IO update failed" + echo "[init] PX4IO update failed" echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if hw_ver compare PX4FMU_V2 + then + tone_alarm MNGGG + fi fi fi - + + set HIL no + set FRAME_TYPE none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + set EXIT_ON_END no + if param compare SYS_AUTOCONFIG 1 + then + set DO_AUTOCONFIG yes + else + set DO_AUTOCONFIG no + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart + + # Custom configuration + if [ -f /fs/microsd/etc/rc.conf ] + then + sh /fs/microsd/etc/rc.conf + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi # # If autoconfig parameter was set, reset it and save parameters @@ -240,66 +213,132 @@ then param set SYS_AUTOCONFIG 0 param save fi - + if [ $MODE == autostart ] then # - # Do common setup depending on env variables + # Start primary output # - if [ $USE_IO == yes ] + if [ $OUTPUT_MODE == io_pwm ] then - echo "Use IO" - - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - else - echo "Don't use IO" - - # Start MAVLink on ttyS0 + echo "[init] Use PX4IO PWM as primary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + fmu mode_pwm + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + mkblctrl + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + hil mode_pwm + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start + then + echo "[init] PX4IO OK" + echo "PX4IO OK" >> $logfile + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + echo "PX4IO start error" >> $logfile + tone_alarm MNGGG + fi + fi + + # + # MAVLink + # + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output mavlink start -d /dev/ttyS0 usleep 5000 - # Configure FMU for PWM outputs - fmu mode_pwm - # Exit from nsh to free port for mavlink set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi - # Sensors - echo "Start sensors" + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" sh /etc/init.d/rc.sensors - - # Logging - sh /etc/init.d/rc.logging - - # GPS interface - gps start + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + fi + + if [ $HIL == no ] + then + echo "[init] Start GPS" + gps start + fi # # Fixed wing setup # - if [ $FRAME_TYPE == fw ] + if [ $VEHICLE_TYPE == fw ] then - echo "Setup FIXED WING" + echo "[init] Vehicle type: FIXED WING" + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps fi # # Multicopters setup # - if [ $FRAME_TYPE == mc ] + if [ $VEHICLE_TYPE == mc ] then - echo "Setup MULTICOPTER" + echo "[init] Vehicle type: MULTICOPTER" # Load mixer and configure outputs sh /etc/init.d/rc.mc_interface - # Start common multicopter apps + # Start standard multicopter apps sh /etc/init.d/rc.mc_apps fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start + position_estimator_inav start + fi fi # Start any custom extensions @@ -307,39 +346,6 @@ then then sh /fs/microsd/etc/rc.local fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect - then - px4io start - else - if fmu mode_serial - then - echo "FMU driver (no PWM) started" - fi - fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - - fi if [ $EXIT_ON_END == yes ] then -- cgit v1.2.3 From 6e609845569722367b5d38bc508edb69dfa8d47f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 22:04:56 +0100 Subject: rcS and autostart scripts cleanup, WIP --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 9 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 15 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 43 ------ ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 46 ++++++ ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 133 +++++------------ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 7 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 7 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 9 +- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- ROMFS/px4fmu_common/init.d/rcS | 199 ++++++++++++++----------- 10 files changed, 223 insertions(+), 249 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 610d482c1..24f1099d3 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,8 +1,9 @@ #!nsh - -# Maintainers: Anton Babushkin - -echo "[init] Team Blacksheep Discovery Quad" +# +# Team Blacksheep Discovery Quadcopter +# +# Maintainers: Simon Wilks +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index de5028052..b8fc5e606 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,11 +1,9 @@ #!nsh - +# +# 3DR Iris Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] 3DR Iris Quad" - -# Use generic wide arm quad X PWM as base -sh /etc/init.d/10001_quad_w +# if [ $DO_AUTOCONFIG == yes ] then @@ -41,3 +39,8 @@ then param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 fi + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_w + +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 7d21176f2..000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,43 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on HIL Quad" - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 -fi - -set HIL yes - -set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 000000000..c5b92d7d4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,46 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 +fi + +set HIL yes + +set VEHICLE_TYPE mc +set FRAME_GEOMETRY 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 0cc07ad34..5ec70043a 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,46 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + # - -echo "[HIL] HILS quadrotor + starting.." - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# Maintainers: Anton Babushkin # -param set MAV_TYPE 2 -# -# Check if we got an IO -# -if px4io start +if [ $DO_AUTOCONFIG == yes ] then - echo "IO started" -else - fmu mode_serial - echo "FMU started" + # + # Default parameters for this platform + # + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.8 + param set MPC_THR_MIN 0.2 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 fi -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE mc +set FRAME_GEOMETRY quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 0cfe68b27..94afe91ae 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F330 Quadcopter +# # Maintainers: Anton Babushkin - -echo "[init] DJI F330" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 4a0953a2e..21b3088d3 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,9 @@ #!nsh - +# +# DJI Flame Wheel F450 Quadcopter +# # Maintainers: Lorenz Meier - -echo "[init] DJI F450" +# if [ $DO_AUTOCONFIG == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 396349d9c..27f73471d 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -1,8 +1,9 @@ #!nsh - +# +# HobbyKing X550 Quadcopter +# # Maintainers: Todd Stellanova - -echo "[init] PX4FMU v1, v2 with or without IO on HobbyKing X550" +# if [ $DO_AUTOCONFIG == yes ] then @@ -21,6 +22,8 @@ then param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9da0135b3..352a916ac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -28,7 +28,7 @@ fi if param compare SYS_AUTOSTART 1001 then - sh /etc/init.d/1001_rc_quad.hil + sh /etc/init.d/1001_rc_quad_x.hil fi if param compare SYS_AUTOSTART 1002 @@ -38,7 +38,7 @@ fi if param compare SYS_AUTOSTART 1003 then - #sh /etc/init.d/1003_rc_quad_+.hil + sh /etc/init.d/1003_rc_quad_+.hil fi if param compare SYS_AUTOSTART 1004 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 09d66cf41..d6d6ec352 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,18 +13,31 @@ set logfile /fs/microsd/bootlog.txt # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[init] Looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[init] microSD card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[init] No microSD card found" # Play SOS tone_alarm error fi +# +# Set default values here, can be overriden in rc.txt from SD card +# +set HIL no +set VEHICLE_TYPE none +set FRAME_GEOMETRY none +set OUTPUT_MODE none +set VEHICLE_TYPE none +set PWM_RATE none +set PWM_DISARMED none +set PWM_MIN none +set PWM_MAX none + # # Look for an init script on the microSD card. # @@ -70,27 +83,18 @@ then uorb start # - # Load microSD params + # Load parameters # - #if ramtron start - #then - # param select /ramtron/params - # if [ -f /ramtron/params ] - # then - # param load /ramtron/params - # fi - #else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + if param load /fs/microsd/params then - if param load /fs/microsd/params - then - echo "[init] Parameters loaded" - else - echo "[init] Parameter file corrupt - ignoring" - fi + echo "[init] Parameters loaded" + else + echo "[init] Parameter file corrupt - ignoring" fi - #fi + fi # # Start system state indicator @@ -104,13 +108,9 @@ then blinkm systemstate fi fi - - # Use FMU PWM output by default - set OUTPUT_MODE fmu_pwm - set IO_PRESENT no # - # Upgrade PX4IO firmware + # Check if PX4IO present and update firmware if needed # if [ -f /etc/extras/px4io-v2_default.bin ] then @@ -119,15 +119,14 @@ then set io_file /etc/extras/px4io-v1_default.bin fi + set IO_PRESENT no + if px4io checkcrc $io_file then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $logfile set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO CRC failure" echo "PX4IO CRC failure" >> $logfile @@ -142,9 +141,6 @@ then tone_alarm MSPAA set IO_PRESENT yes - - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm else echo "[init] PX4IO restart failed" echo "PX4IO restart failed" >> $logfile @@ -164,16 +160,27 @@ then fi fi fi - - set HIL no - set FRAME_TYPE none - set PWM_RATE none - set PWM_DISARMED none - set PWM_MIN none - set PWM_MAX none + + # + # Set default output if it was not defined in rc.txt + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm + fi + fi set EXIT_ON_END no + # + # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts + # if param compare SYS_AUTOCONFIG 1 then set DO_AUTOCONFIG yes @@ -181,41 +188,35 @@ then set DO_AUTOCONFIG no fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - # # Set parameters and env variables for selected AUTOSTART # sh /etc/init.d/rc.autostart - # Custom configuration - if [ -f /fs/microsd/etc/rc.conf ] - then - sh /fs/microsd/etc/rc.conf - fi - - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi - if [ $MODE == autostart ] then + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + # # Start primary output # @@ -224,29 +225,47 @@ then echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi if [ $OUTPUT_MODE == fmu_pwm ] then echo "[init] Use PX4FMU PWM as primary output" - fmu mode_pwm + if fmu mode_pwm + then + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - mkblctrl + if mkblctrl + then + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG + fi + fi if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - hil mode_pwm + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG + fi fi # @@ -257,12 +276,10 @@ then echo "[init] Use PX4IO PWM as secondary output" if px4io start then - echo "[init] PX4IO OK" - echo "PX4IO OK" >> $logfile + echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - echo "PX4IO start error" >> $logfile tone_alarm MNGGG fi fi @@ -270,18 +287,24 @@ then # # MAVLink # - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 + mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi fi # @@ -294,10 +317,7 @@ then then echo "[init] Start logging" sh /etc/init.d/rc.logging - fi - - if [ $HIL == no ] - then + echo "[init] Start GPS" gps start fi @@ -336,6 +356,7 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" + attitude_estimator_ekf start position_estimator_inav start fi -- cgit v1.2.3 From f3a3d62cf94e85d69243715de4d5e0cf70d2dbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:14:29 +0100 Subject: Use rc.txt, config.txt, extras.txt files, minor boot messages fixes --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 12 +- ROMFS/px4fmu_common/init.d/rcS | 420 +++++++++++++++-------------- 3 files changed, 232 insertions(+), 210 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index f9517f422..c864e7c61 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load fixedwing mixer and set rate/disarmed/min/max values for PWM output +# Script to configure fixedwing control interface # # @@ -12,7 +12,7 @@ param set MAV_TYPE 1 # # Load mixer # -echo "Frame geometry: ${FRAME_GEOMETRY}" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: ${MIXER}" -mixer load /dev/pwm_output ${MIXER} +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" +mixer load /dev/pwm_output $MIXER diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 003aee81b..7fbcbd282 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -1,6 +1,6 @@ #!nsh # -# Script to load multicopter mixer and set rate/disarmed/min/max values for PWM output +# Script to configure multicopter control interface # if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] @@ -27,9 +27,9 @@ fi # # Load mixer # -echo "Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_${FRAME_GEOMETRY}.mix -echo "Loading mixer: $MIXER" +echo "[init] Frame geometry: $FRAME_GEOMETRY" +set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +echo "[init] Loading mixer: $MIXER" mixer load /dev/pwm_output $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] @@ -39,6 +39,7 @@ then # if [ $PWM_RATE != none ] then + echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $OUTPUTS -r $PWM_RATE fi @@ -47,14 +48,17 @@ then # if [ $PWM_DISARMED != none ] then + echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then + echo "[init] Set PWM min: $PWM_MIN" pwm min -c $OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then + echo "[init] Set PWM max: $PWM_MAX" pwm max -c $OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d6d6ec352..576fc2d94 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,10 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set LOG_FILE /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt # # Try to mount the microSD card. @@ -25,35 +28,17 @@ else tone_alarm error fi -# -# Set default values here, can be overriden in rc.txt from SD card -# -set HIL no -set VEHICLE_TYPE none -set FRAME_GEOMETRY none -set OUTPUT_MODE none -set VEHICLE_TYPE none -set PWM_RATE none -set PWM_DISARMED none -set PWM_MIN none -set PWM_MAX none - # # Look for an init script on the microSD card. +# Disable autostart if the script found. # -# To prevent automatic startup in the current flight mode, -# the script should set MODE to some other value. -# -if [ -f /fs/microsd/etc/rc ] -then - echo "[init] reading /fs/microsd/etc/rc" - sh /fs/microsd/etc/rc -fi -# Also consider rc.txt files -if [ -f /fs/microsd/etc/rc.txt ] +if [ -f $RC_FILE ] then - echo "[init] reading /fs/microsd/etc/rc.txt" - sh /fs/microsd/etc/rc.txt + echo "[init] Executing init script: $RC_FILE" + sh $RC_FILE + set MODE custom +else + echo "[init] Init script not found: $RC_FILE" fi # if this is an APM build then there will be a rc.APM script @@ -72,15 +57,7 @@ fi if [ $MODE == autostart ] then - # - # Start CDC/ACM serial driver - # - sercon - - # - # Start the ORB (first app to start) - # - uorb start + echo "[init] AUTOSTART mode" # # Load parameters @@ -95,6 +72,16 @@ then echo "[init] Parameter file corrupt - ignoring" fi fi + + # + # Start CDC/ACM serial driver + # + sercon + + # + # Start the ORB (first app to start) + # + uorb start # # Start system state indicator @@ -114,36 +101,36 @@ then # if [ -f /etc/extras/px4io-v2_default.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2_default.bin else - set io_file /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1_default.bin fi set IO_PRESENT no - if px4io checkcrc $io_file + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile + echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile + echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if px4io forceupdate 14662 $IO_FILE then usleep 500000 if px4io start then echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile + echo "PX4IO restart OK" >> $LOG_FILE tone_alarm MSPAA set IO_PRESENT yes else echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile + echo "PX4IO restart failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG @@ -153,27 +140,35 @@ then fi else echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $logfile + echo "PX4IO update failed" >> $LOG_FILE if hw_ver compare PX4FMU_V2 then tone_alarm MNGGG fi fi fi - + # - # Set default output if it was not defined in rc.txt + # Set default values # - if [ $OUTPUT_MODE == none ] + set HIL no + set VEHICLE_TYPE none + set FRAME_GEOMETRY none + set PWM_RATE none + set PWM_DISARMED none + set PWM_MIN none + set PWM_MAX none + + # + # Set default output + # + if [ $IO_PRESENT == yes ] then - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io_pwm + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu_pwm fi set EXIT_ON_END no @@ -193,179 +188,202 @@ then # sh /etc/init.d/rc.autostart - if [ $MODE == autostart ] - then - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - - # - # If autoconfig parameter was set, reset it and save parameters - # - if [ $DO_AUTOCONFIG == yes ] - then - param set SYS_AUTOCONFIG 0 - param save - fi + # + # Override parameters from user configuration file + # + if [ -f $CONFIG_FILE ] + then + echo "[init] Reading config: $CONFIG_FILE" + sh $CONFIG_FILE + else + echo "[init] Config file not found: $CONFIG_FILE" + fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # - commander start - - # - # Start primary output - # - if [ $OUTPUT_MODE == io_pwm ] + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + + # + # If autoconfig parameter was set, reset it and save parameters + # + if [ $DO_AUTOCONFIG == yes ] + then + param set SYS_AUTOCONFIG 0 + param save + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start + + # + # Start primary output + # + if [ $OUTPUT_MODE == io_pwm ] + then + echo "[init] Use PX4IO PWM as primary output" + if px4io start then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG fi - if [ $OUTPUT_MODE == fmu_pwm ] + fi + if [ $OUTPUT_MODE == fmu_pwm ] + then + echo "[init] Use PX4FMU PWM as primary output" + if fmu mode_pwm then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm - then - echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io - else - echo "[init] PX4FMU PWM output start error" - tone_alarm MNGGG - fi + echo "[init] PX4FMU PWM output started" + sh /etc/init.d/rc.io + else + echo "[init] PX4FMU PWM output start error" + tone_alarm MNGGG fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - if mkblctrl - then - echo "[init] MKBLCTRL started" - else - echo "[init] MKBLCTRL start error" - tone_alarm MNGGG - fi - - fi - if [ $OUTPUT_MODE == hil ] + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + if mkblctrl then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] HIL output error" - tone_alarm MNGGG - fi + echo "[init] MKBLCTRL started" + else + echo "[init] MKBLCTRL start error" + tone_alarm MNGGG fi - # - # Start PX4IO as secondary output if needed - # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG - fi + echo "[init] HIL output started" + else + echo "[init] HIL output error" + tone_alarm MNGGG fi - - # - # MAVLink - # - if [ $HIL == yes ] + fi + + # + # Start PX4IO as secondary output if needed + # + if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + then + echo "[init] Use PX4IO PWM as secondary output" + if px4io start then - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 + echo "[init] PX4IO started" + sh /etc/init.d/rc.io else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] - then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output - mavlink start -d /dev/ttyS0 - usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes - else - # Start MAVLink on default port: ttyS1 - mavlink start - usleep 5000 - fi + echo "[init] PX4IO start error" + tone_alarm MNGGG fi - - # - # Sensors, Logging, GPS - # - echo "[init] Start sensors" - sh /etc/init.d/rc.sensors + fi - if [ $HIL == no ] + # + # MAVLink + # + if [ $HIL == yes ] + then + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] then - echo "[init] Start logging" - sh /etc/init.d/rc.logging + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + mavlink start -d /dev/ttyS0 + usleep 5000 - echo "[init] Start GPS" - gps start + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 fi + fi + + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging - # - # Fixed wing setup - # - if [ $VEHICLE_TYPE == fw ] - then - echo "[init] Vehicle type: FIXED WING" - - # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface - - # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps - fi + echo "[init] Start GPS" + gps start + fi - # - # Multicopters setup - # - if [ $VEHICLE_TYPE == mc ] + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "[init] Vehicle type: FIXED WING" + + if [ $FRAME_GEOMETRY == none ] then - echo "[init] Vehicle type: MULTICOPTER" - - # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface - - # Start standard multicopter apps - sh /etc/init.d/rc.mc_apps + # Set default frame geometry for fixed wing + set FRAME_GEOMETRY AERT fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.fw_interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.mc_apps + fi - # - # Generic setup (autostart ID not found) - # - if [ $VEHICLE_TYPE == none ] + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "[init] Vehicle type: MULTICOPTER" + + if [ $FRAME_GEOMETRY == none ] then - echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + # Set default frame geometry for multicopter + set FRAME_GEOMETRY quad_x fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.mc_interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps fi - # Start any custom extensions - if [ -f /fs/microsd/etc/rc.local ] + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] then - sh /fs/microsd/etc/rc.local + echo "[init] Vehicle type: GENERIC" + + attitude_estimator_ekf start + position_estimator_inav start + fi + + # Start any custom addons + if [ -f $EXTRAS_FILE ] + then + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] -- cgit v1.2.3 From 2e1199299219baf487b4c31ebf4453ffaf971fbf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 10 Jan 2014 23:41:03 +0100 Subject: Don’t try to find autostart script if SYS_AUTOSTART = 0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 576fc2d94..178ed1812 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -186,7 +186,12 @@ then # # Set parameters and env variables for selected AUTOSTART # - sh /etc/init.d/rc.autostart + if param compare SYS_AUTOSTART 0 + then + echo "[init] Don't try to find autostart script" + else + sh /etc/init.d/rc.autostart + fi # # Override parameters from user configuration file -- cgit v1.2.3 From e301bdcf99271a6c95bbe6b83ab7b24ba682b211 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:34:46 +0100 Subject: Autostart: some fixed wing setups added --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 6 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 9 ++- ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 -------------------------- ROMFS/px4fmu_common/init.d/3031_phantom | 44 +++++++++++++ ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 ------------------------- ROMFS/px4fmu_common/init.d/3033_wingwing | 43 +++++++++++++ ROMFS/px4fmu_common/init.d/3034_fx79 | 43 +++++++++++++ ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 ------------------------- ROMFS/px4fmu_common/init.d/rc.autostart | 14 ++--- 9 files changed, 145 insertions(+), 267 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3031_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3033_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 24f1099d3..53140caff 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -24,9 +24,7 @@ then param set MC_YAWRATE_D 0.0 fi -set FRAME_TYPE mc +set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w + set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1100 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 9bf01c60c..43f911a78 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,6 +1,9 @@ #!nsh - -echo "[init] EasyStar" +# +# MPX EasyStar Plane +# +# Maintainers: ??? +# if [ $DO_AUTOCONFIG == yes ] then @@ -32,5 +35,5 @@ then param set RC_SCALE_PITCH 1.0 fi -set FRAME_TYPE fw +set VEHICLE_TYPE fw set FRAME_GEOMETRY RET diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a..000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 000000000..29af48ec6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6..000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 000000000..e0340a8d9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 000000000..c4dab7ba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb4..000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 352a916ac..69e88fcd0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -77,24 +77,24 @@ then #sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 31 +if param compare SYS_AUTOSTART 3031 then - #sh /etc/init.d/3031_io_phantom + sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 32 +if param compare SYS_AUTOSTART 3032 then #sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 33 +if param compare SYS_AUTOSTART 3033 then - #sh /etc/init.d/3033_io_wingwing + sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 34 +if param compare SYS_AUTOSTART 3034 then - #sh /etc/init.d/3034_io_fx79 + sh /etc/init.d/3034_fx79 fi # -- cgit v1.2.3 From 3f6f26e5f6bb89056a0b07b4761fdf81f0f8491a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 00:40:19 +0100 Subject: Autostart for fixed wing bug fixed --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 178ed1812..c92876f26 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -348,7 +348,7 @@ then sh /etc/init.d/rc.fw_interface # Start standard fixedwing apps - sh /etc/init.d/rc.mc_apps + sh /etc/init.d/rc.fw_apps fi # -- cgit v1.2.3 From 95c99618f9f22ba08b4826a98c32f995ea3bcf17 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:26:55 +0100 Subject: Autostart: mkblctrl mixer loading fixed --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 7fbcbd282..401ff775f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -30,7 +30,13 @@ fi echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER +if [ $OUTPUT_MODE == mkblctrl ] +then + set OUTPUT_DEV /dev/mkblctrl +else + set OUTPUT_DEV /dev/pwm_output +fi +mixer load $OUTPUT_DEV $MIXER if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] then -- cgit v1.2.3 From 41add86164e15d553a5b1d2d2f9d55d964ca4ebe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 11:34:18 +0100 Subject: Autostart: standalone FMUv1 setup fixed --- ROMFS/px4fmu_common/init.d/rcS | 1 - 1 file changed, 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c92876f26..f8796bb08 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -247,7 +247,6 @@ then if fmu mode_pwm then echo "[init] PX4FMU PWM output started" - sh /etc/init.d/rc.io else echo "[init] PX4FMU PWM output start error" tone_alarm MNGGG -- cgit v1.2.3 From 4dd5c13b98f57e490274d7b3fb6d297e18e79853 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 17:52:29 +0100 Subject: Autostart: fixes --- ROMFS/px4fmu_common/init.d/rc.mc_interface | 2 +- ROMFS/px4fmu_common/init.d/rcS | 238 ++++++++++++++++++----------- 2 files changed, 146 insertions(+), 94 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 401ff775f..12be3a931 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -38,7 +38,7 @@ else fi mixer load $OUTPUT_DEV $MIXER -if [ $OUTPUT_MODE == fmu_pwm -o $OUTPUT_MODE == io_pwm ] +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then # # Set PWM output frequency diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f8796bb08..5e13e4fa2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -96,82 +96,20 @@ then fi fi - # - # Check if PX4IO present and update firmware if needed - # - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set IO_FILE /etc/extras/px4io-v2_default.bin - else - set IO_FILE /etc/extras/px4io-v1_default.bin - fi - - set IO_PRESENT no - - if px4io checkcrc $IO_FILE - then - echo "[init] PX4IO CRC OK" - echo "PX4IO CRC OK" >> $LOG_FILE - - set IO_PRESENT yes - else - echo "[init] PX4IO CRC failure" - echo "PX4IO CRC failure" >> $LOG_FILE - tone_alarm MBABGP - if px4io forceupdate 14662 $IO_FILE - then - usleep 500000 - if px4io start - then - echo "[init] PX4IO restart OK" - echo "PX4IO restart OK" >> $LOG_FILE - tone_alarm MSPAA - - set IO_PRESENT yes - else - echo "[init] PX4IO restart failed" - echo "PX4IO restart failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - sleep 10 - reboot - fi - fi - else - echo "[init] PX4IO update failed" - echo "PX4IO update failed" >> $LOG_FILE - if hw_ver compare PX4FMU_V2 - then - tone_alarm MNGGG - fi - fi - fi - # # Set default values # set HIL no set VEHICLE_TYPE none set FRAME_GEOMETRY none + set USE_IO yes + set OUTPUT_MODE none set PWM_RATE none set PWM_DISARMED none set PWM_MIN none set PWM_MAX none - - # - # Set default output - # - if [ $IO_PRESENT == yes ] - then - # If PX4IO present, use it as primary PWM output by default - set OUTPUT_MODE io_pwm - else - # Else use PX4FMU PWM output - set OUTPUT_MODE fmu_pwm - fi - - set EXIT_ON_END no + set MKBLCTRL_MODE none + set FMU_MODE pwm # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -182,7 +120,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -204,14 +142,6 @@ then echo "[init] Config file not found: $CONFIG_FILE" fi - if [ $HIL == yes ] - then - set OUTPUT_MODE hil - else - # Try to get an USB console if not in HIL mode - nshterm /dev/ttyACM0 & - fi - # # If autoconfig parameter was set, reset it and save parameters # @@ -220,7 +150,82 @@ then param set SYS_AUTOCONFIG 0 param save fi + + set IO_PRESENT no + + if [ $USE_IO == yes ] + then + # + # Check if PX4IO present and update firmware if needed + # + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set IO_FILE /etc/extras/px4io-v2_default.bin + else + set IO_FILE /etc/extras/px4io-v1_default.bin + fi + + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK" + echo "PX4IO CRC OK" >> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] PX4IO CRC failure" + echo "PX4IO CRC failure" >> $LOG_FILE + tone_alarm MBABGP + if px4io forceupdate 14662 $IO_FILE + then + usleep 500000 + if px4io checkcrc $IO_FILE + then + echo "[init] PX4IO CRC OK after updating" + echo "PX4IO CRC OK after updating" >> $LOG_FILE + tone_alarm MSPAA + + set IO_PRESENT yes + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + else + echo "[init] PX4IO update failed" + echo "PX4IO update failed" >> $LOG_FILE + fi + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" + tone_alarm MNGGG + set VEHICLE_TYPE none + fi + fi + + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] + then + if [ $IO_PRESENT == yes ] + then + # If PX4IO present, use it as primary PWM output by default + set OUTPUT_MODE io + else + # Else use PX4FMU PWM output + set OUTPUT_MODE fmu + fi + fi + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + # # Start the Commander (needs to be this early for in-air-restarts) # @@ -229,7 +234,9 @@ then # # Start primary output # - if [ $OUTPUT_MODE == io_pwm ] + set TTYS1_BUSY no + + if [ $OUTPUT_MODE == io ] then echo "[init] Use PX4IO PWM as primary output" if px4io start @@ -241,21 +248,43 @@ then tone_alarm MNGGG fi fi - if [ $OUTPUT_MODE == fmu_pwm ] + if [ $OUTPUT_MODE == fmu ] then - echo "[init] Use PX4FMU PWM as primary output" - if fmu mode_pwm + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE then - echo "[init] PX4FMU PWM output started" + echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] PX4FMU PWM output start error" + echo "[init] FMU mode_$FMU_MODE start error" tone_alarm MNGGG fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" - if mkblctrl + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" else @@ -277,32 +306,55 @@ then fi # - # Start PX4IO as secondary output if needed + # Start IO or FMU for RC PPM input if needed # - if [ $IO_PRESENT == yes -a $OUTPUT_MODE != io_pwm ] + if [ $IO_PRESENT == yes ] then - echo "[init] Use PX4IO PWM as secondary output" - if px4io start + if [ $OUTPUT_MODE != io ] then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] PX4IO start error" - tone_alarm MNGGG + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] PX4IO start error" + tone_alarm MNGGG + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] FMU mode_$FMU_MODE start error" + tone_alarm MNGGG + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi fi fi # # MAVLink # + set EXIT_ON_END no + if [ $HIL == yes ] then mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else - if [ $OUTPUT_MODE == fmu_pwm -a hw_ver compare PX4FMU_V1 ] + if [ $TTYS1_BUSY == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as PWM output + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else mavlink start -d /dev/ttyS0 usleep 5000 -- cgit v1.2.3 From 3339edeae6bdb4119179eeacc34ab91c0b8aced1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 11 Jan 2014 18:45:56 +0100 Subject: Autostart: tones cleanup --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 10 ++++++++-- ROMFS/px4fmu_common/init.d/rc.mc_interface | 11 +++++++++-- ROMFS/px4fmu_common/init.d/rcS | 24 +++++++++++++----------- 3 files changed, 30 insertions(+), 15 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index c864e7c61..69f3ed7f7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -14,5 +14,11 @@ param set MAV_TYPE 1 # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" -mixer load /dev/pwm_output $MIXER + +if mixer load /dev/pwm_output $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface index 12be3a931..6e4e4ed31 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ b/ROMFS/px4fmu_common/init.d/rc.mc_interface @@ -29,14 +29,21 @@ fi # echo "[init] Frame geometry: $FRAME_GEOMETRY" set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -echo "[init] Loading mixer: $MIXER" + if [ $OUTPUT_MODE == mkblctrl ] then set OUTPUT_DEV /dev/mkblctrl else set OUTPUT_DEV /dev/pwm_output fi -mixer load $OUTPUT_DEV $MIXER + +if mixer load $OUTPUT_DEV $MIXER +then + echo "[init] Mixer loaded: $MIXER" +else + echo "[init] Error loading mixer: $MIXER" + tone_alarm $TUNE_OUT_ERROR +fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5e13e4fa2..a809f2870 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -13,6 +13,7 @@ set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt +set TUNE_OUT_ERROR ML<> $LOG_FILE - tone_alarm MBABGP + + tone_alarm MLL32CP8MB + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -182,7 +185,7 @@ then then echo "[init] PX4IO CRC OK after updating" echo "PX4IO CRC OK after updating" >> $LOG_FILE - tone_alarm MSPAA + tone_alarm MLL8CDE set IO_PRESENT yes else @@ -197,9 +200,8 @@ then if [ $IO_PRESENT == no ] then - echo "[init] ERROR: PX4IO not found, set vehicle type to NONE" - tone_alarm MNGGG - set VEHICLE_TYPE none + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR fi fi @@ -245,7 +247,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi if [ $OUTPUT_MODE == fmu ] @@ -256,7 +258,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 @@ -289,7 +291,7 @@ then echo "[init] MKBLCTRL started" else echo "[init] MKBLCTRL start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -301,7 +303,7 @@ then echo "[init] HIL output started" else echo "[init] HIL output error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi @@ -318,7 +320,7 @@ then sh /etc/init.d/rc.io else echo "[init] PX4IO start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi fi else @@ -329,7 +331,7 @@ then echo "[init] FMU mode_$FMU_MODE started" else echo "[init] FMU mode_$FMU_MODE start error" - tone_alarm MNGGG + tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 -- cgit v1.2.3 From d079074e801a6cb590f91053e2e6d96d5113c5c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:49:43 +0100 Subject: update the skywalker x5 script --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 56 +++------------------------- 1 file changed, 6 insertions(+), 50 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e041654..bda05aeb1 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,14 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - -# -# Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - +# Skywalker X5 Flying Wing # -# Start and configure PX4IO or FMU interface +# Maintainers: Thomas Gubler , Julian Oes # -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +if [ $DO_AUTOCONFIG == yes ] then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix + # TODO fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set FRAME_GEOMETRY Q -- cgit v1.2.3 From 449ed058c3554d2317e25395a2f5ebdb21816294 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 12 Jan 2014 18:55:41 +0100 Subject: rc.fw_interface: use mixer file from sd if it exists --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 69f3ed7f7..1bf7a0e03 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -13,7 +13,17 @@ param set MAV_TYPE 1 # Load mixer # echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix + +#Use the mixer file from the sd-card if it exists +if [ -f $MIXERSD ] +then + set MIXER MIXERSD +else + set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix +fi + + if mixer load /dev/pwm_output $MIXER then -- cgit v1.2.3 From 1008d0c38320544579eb449d54c053e14568585e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 13 Jan 2014 08:03:38 +0100 Subject: fix small error in rc.fw_interface --- ROMFS/px4fmu_common/init.d/rc.fw_interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface index 1bf7a0e03..133b65218 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ b/ROMFS/px4fmu_common/init.d/rc.fw_interface @@ -18,7 +18,7 @@ set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix #Use the mixer file from the sd-card if it exists if [ -f $MIXERSD ] then - set MIXER MIXERSD + set MIXER $MIXERSD else set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix fi -- cgit v1.2.3 From d1b2186806e0b9e32808a04f6c85d26a703c596e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 14 Jan 2014 21:59:48 +0100 Subject: Autostart 3DR Iris updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b8fc5e606..6740e2d94 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -38,9 +38,16 @@ then param set MPC_Z_VEL_I 0.1 param set MPC_Z_VEL_MAX 2 param set MPC_Z_VEL_P 0.20 + + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi set VEHICLE_TYPE mc set FRAME_GEOMETRY quad_w set PWM_RATE 400 + +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 -- cgit v1.2.3 From a8d362de13b23a2523dc69d582c68fe672ac236d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 15 Jan 2014 00:02:57 +0100 Subject: Autostart: use MIXER instead of FRAME_GEOMETRY --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 3 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 3 +- ROMFS/px4fmu_common/init.d/rc.fw_interface | 34 ------- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++++++++++++++ ROMFS/px4fmu_common/init.d/rc.io | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_interface | 77 --------------- ROMFS/px4fmu_common/init.d/rc.sensors | 21 ++-- ROMFS/px4fmu_common/init.d/rcS | 131 +++++++++++++++++-------- 18 files changed, 195 insertions(+), 182 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 53140caff..63798bb3c 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -25,6 +25,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6740e2d94..67c24fab3 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -44,10 +44,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_w +set MIXER FMU_quad_w +set PWM_OUTPUTS 1234 set PWM_RATE 400 - set PWM_DISARMED 900 set PWM_MIN 1000 set PWM_MAX 2000 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 c5b92d7d4..8c0797d7c 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_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 5ec70043a..bce3015fc 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -43,4 +43,4 @@ fi set HIL yes set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_+ +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 43f911a78..0e5bf60d6 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -36,4 +36,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY RET +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 29af48ec6..4ebbe9c61 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -41,4 +41,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index bda05aeb1..03f282237 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -11,4 +11,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e0340a8d9..e53763278 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index c4dab7ba6..8d179d1fd 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -40,4 +40,4 @@ then fi set VEHICLE_TYPE fw -set FRAME_GEOMETRY Q +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 94afe91ae..ab1db94d0 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -41,10 +41,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 21b3088d3..299771c1d 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -27,10 +27,10 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 - # DJI ESC range set PWM_DISARMED 900 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 27f73471d..7e020cf59 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -27,6 +27,7 @@ then fi set VEHICLE_TYPE mc -set FRAME_GEOMETRY quad_x +set MIXER FMU_quad_x +set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_interface b/ROMFS/px4fmu_common/init.d/rc.fw_interface deleted file mode 100644 index 133b65218..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fw_interface +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Script to configure fixedwing control interface -# - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXERSD /fs/microsd/etc/mixers/FMU_$FRAME_GEOMETRY.mix - -#Use the mixer file from the sd-card if it exists -if [ -f $MIXERSD ] -then - set MIXER $MIXERSD -else - set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix -fi - - - -if mixer load /dev/pwm_output $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 000000000..928d3aeeb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + if [ $PWM_DISARMED != none ] + then + echo "[init] Set PWM disarmed: $PWM_DISARMED" + pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + fi + if [ $PWM_MIN != none ] + then + echo "[init] Set PWM min: $PWM_MIN" + pwm min -c $PWM_OUTPUTS -p $PWM_MIN + fi + if [ $PWM_MAX != none ] + then + echo "[init] Set PWM max: $PWM_MAX" + pwm max -c $PWM_OUTPUTS -p $PWM_MAX + fi + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 5a010cc9b..c9d964f8e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -9,11 +9,13 @@ px4io recovery # -# Adjust px4io topic limiting +# Adjust PX4IO update rate limit # +set PX4IO_LIMIT 400 if hw_ver compare PX4FMU_V1 then - px4io limit 200 -else - px4io limit 400 + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6e4e4ed31..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,77 +0,0 @@ -#!nsh -# -# Script to configure multicopter control interface -# - -if [ $FRAME_GEOMETRY == quad_x -o $FRAME_GEOMETRY == quad_+ ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == quad_w -o $FRAME_GEOMETRY == quad_v ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -fi -if [ $FRAME_GEOMETRY == hex_x -o $FRAME_GEOMETRY == hex_+ ] -then - set OUTPUTS 123456 - param set MAV_TYPE 13 -fi -if [ $FRAME_GEOMETRY == octo_x -o $FRAME_GEOMETRY == octo_+ ] -then - set OUTPUTS 12345678 - param set MAV_TYPE 14 -fi - -# -# Load mixer -# -echo "[init] Frame geometry: $FRAME_GEOMETRY" -set MIXER /etc/mixers/FMU_$FRAME_GEOMETRY.mix - -if [ $OUTPUT_MODE == mkblctrl ] -then - set OUTPUT_DEV /dev/mkblctrl -else - set OUTPUT_DEV /dev/pwm_output -fi - -if mixer load $OUTPUT_DEV $MIXER -then - echo "[init] Mixer loaded: $MIXER" -else - echo "[init] Error loading mixer: $MIXER" - tone_alarm $TUNE_OUT_ERROR -fi - -if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] -then - # - # Set PWM output frequency - # - if [ $PWM_RATE != none ] - then - echo "[init] Set PWM rate: $PWM_RATE" - pwm rate -c $OUTPUTS -r $PWM_RATE - fi - - # - # Set disarmed, min and max PWM values - # - if [ $PWM_DISARMED != none ] - then - echo "[init] Set PWM disarmed: $PWM_DISARMED" - pwm disarmed -c $OUTPUTS -p $PWM_DISARMED - fi - if [ $PWM_MIN != none ] - then - echo "[init] Set PWM min: $PWM_MIN" - pwm min -c $OUTPUTS -p $PWM_MIN - fi - if [ $PWM_MAX != none ] - then - echo "[init] Set PWM max: $PWM_MAX" - pwm max -c $OUTPUTS -p $PWM_MAX - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index a2517135f..badbf92c3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,39 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - echo "using LSM303D" + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 441d99ecf..92121ac17 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -14,6 +14,7 @@ set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt set TUNE_OUT_ERROR ML<> $LOG_FILE tone_alarm MLL32CP8MB @@ -193,17 +193,17 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK after updating" + echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi else - echo "[init] PX4IO update failed" + echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE fi fi @@ -220,16 +220,27 @@ then # if [ $OUTPUT_MODE == none ] then - if [ $IO_PRESENT == yes ] + if [ $USE_IO == yes ] then - # If PX4IO present, use it as primary PWM output by default set OUTPUT_MODE io else - # Else use PX4FMU PWM output set OUTPUT_MODE fmu fi fi + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + if [ $HIL == yes ] then set OUTPUT_MODE hil @@ -256,7 +267,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -267,7 +278,7 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -300,7 +311,7 @@ then then echo "[init] MKBLCTRL started" else - echo "[init] MKBLCTRL start error" + echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi @@ -312,7 +323,7 @@ then then echo "[init] HIL output started" else - echo "[init] HIL output error" + echo "[init] ERROR: HIL output start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -329,7 +340,7 @@ then echo "[init] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] PX4IO start error" + echo "[init] ERROR: PX4IO start failed" tone_alarm $TUNE_OUT_ERROR fi fi @@ -340,13 +351,17 @@ then then echo "[init] FMU mode_$FMU_MODE started" else - echo "[init] FMU mode_$FMU_MODE start error" + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi if hw_ver compare PX4FMU_V1 then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio -o $FMU_MODE == pwm_gpio ] + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] then set TTYS1_BUSY yes fi @@ -401,14 +416,22 @@ then then echo "[init] Vehicle type: FIXED WING" - if [ $FRAME_GEOMETRY == none ] + if [ $MIXER == none ] then - # Set default frame geometry for fixed wing - set FRAME_GEOMETRY AERT + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT fi + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + # Load mixer and configure outputs - sh /etc/init.d/rc.fw_interface + sh /etc/init.d/rc.interface # Start standard fixedwing apps sh /etc/init.d/rc.fw_apps @@ -420,15 +443,37 @@ then if [ $VEHICLE_TYPE == mc ] then echo "[init] Vehicle type: MULTICOPTER" - - if [ $FRAME_GEOMETRY == none ] + + if [ $MIXER == none ] then - # Set default frame geometry for multicopter - set FRAME_GEOMETRY quad_x + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + then + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 + fi fi + + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs - sh /etc/init.d/rc.mc_interface + sh /etc/init.d/rc.interface # Start standard multicopter apps sh /etc/init.d/rc.mc_apps @@ -440,9 +485,9 @@ then if [ $VEHICLE_TYPE == none ] then echo "[init] Vehicle type: GENERIC" - - attitude_estimator_ekf start - position_estimator_inav start + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface fi # Start any custom addons -- cgit v1.2.3 From c94eb3198aa37e57643fb3373d14ecef46807935 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 Jan 2014 23:15:23 +0100 Subject: rcS: use mtd on FMUv1 too --- ROMFS/px4fmu_common/init.d/rcS | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 92121ac17..32c7991ef 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -75,12 +75,9 @@ then # Load parameters # set PARAM_FILE /fs/microsd/params - if hw_ver compare PX4FMU_V2 + if mtd start then - if mtd start - then - set PARAM_FILE /fs/mtd_params - fi + set PARAM_FILE /fs/mtd_params fi param select $PARAM_FILE -- cgit v1.2.3 From 5db68264c7b1240811d28d04149e4a49891ab423 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:07:25 +0100 Subject: rcS: HIL fixed --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32c7991ef..6325bb94f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,6 +241,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil + set FMU_MODE serial else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -373,6 +374,7 @@ then if [ $HIL == yes ] then + sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 usleep 5000 else -- cgit v1.2.3 From bb8cf0894f01e9889a261d4cd62798048c0cf7f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 16:47:23 +0100 Subject: autostart: HIL and bad PX4IO fixes --- ROMFS/px4fmu_common/init.d/rcS | 165 +++++++++++++++++++++-------------------- 1 file changed, 86 insertions(+), 79 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6325bb94f..a9c5c59ea 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -241,7 +241,10 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - set FMU_MODE serial + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi else # Try to get an USB console if not in HIL mode nshterm /dev/ttyACM0 & @@ -257,82 +260,12 @@ then # set TTYS1_BUSY no - if [ $OUTPUT_MODE == io ] + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "[init] Use PX4IO PWM as primary output" - if px4io start - then - echo "[init] PX4IO started" - sh /etc/init.d/rc.io - else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - if [ $OUTPUT_MODE == fmu ] - then - echo "[init] Use FMU PWM as primary output" - if fmu mode_$FMU_MODE - then - echo "[init] FMU mode_$FMU_MODE started" - else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - if hw_ver compare PX4FMU_V1 - then - if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] - then - set TTYS1_BUSY yes - fi - if [ $FMU_MODE == pwm_gpio ] - then - set TTYS1_BUSY yes - fi - fi - fi - if [ $OUTPUT_MODE == mkblctrl ] - then - echo "[init] Use MKBLCTRL as primary output" - set MKBLCTRL_ARG "" - if [ $MKBLCTRL_MODE == x ] - then - set MKBLCTRL_ARG "-mkmode x" - fi - if [ $MKBLCTRL_MODE == + ] - then - set MKBLCTRL_ARG "-mkmode +" - fi - - if mkblctrl $MKBLCTRL_ARG - then - echo "[init] MKBLCTRL started" - else - echo "[init] ERROR: MKBLCTRL start failed" - tone_alarm $TUNE_OUT_ERROR - fi - - fi - if [ $OUTPUT_MODE == hil ] - then - echo "[init] Use HIL as primary output" - if hil mode_pwm - then - echo "[init] HIL output started" - else - echo "[init] ERROR: HIL output start failed" - tone_alarm $TUNE_OUT_ERROR - fi - fi - - # - # Start IO or FMU for RC PPM input if needed - # - if [ $IO_PRESENT == yes ] - then - if [ $OUTPUT_MODE != io ] + if [ $OUTPUT_MODE == io ] then + echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -342,9 +275,9 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - else - if [ $OUTPUT_MODE != fmu ] + if [ $OUTPUT_MODE == fmu ] then + echo "[init] Use FMU PWM as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -352,7 +285,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if hw_ver compare PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -365,8 +298,82 @@ then fi fi fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + else + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + fi fi - + # # MAVLink # -- cgit v1.2.3 From a9e288dfdd803cf43136a1c413f5fd86d9bd2794 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 20:08:08 +0100 Subject: rc: bug fixed --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 928d3aeeb..d25f01dde 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -33,7 +33,7 @@ then tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] Mixer not defined + echo "[init] Mixer not defined" tone_alarm $TUNE_OUT_ERROR fi -- cgit v1.2.3 From d96e63960c9f951dac02f122ac1e067f7dd3316f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 19 Jan 2014 12:42:43 +0100 Subject: enable new autostart scripts for Rascal HIL and X5 --- .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 68 +++------------------- ROMFS/px4fmu_common/init.d/rc.autostart | 4 +- 2 files changed, 10 insertions(+), 62 deletions(-) (limited to 'ROMFS/px4fmu_common') 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 344d78422..75a00a675 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,8 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start +set HIL yes -echo "[HIL] setup done, running" +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 69e88fcd0..325520dd0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -43,7 +43,7 @@ fi if param compare SYS_AUTOSTART 1004 then - #sh /etc/init.d/1004_rc_fw_Rascal110.hil + sh /etc/init.d/1004_rc_fw_Rascal110.hil fi # @@ -84,7 +84,7 @@ fi if param compare SYS_AUTOSTART 3032 then - #sh /etc/init.d/3032_skywalker_x5 + sh /etc/init.d/3032_skywalker_x5 fi if param compare SYS_AUTOSTART 3033 -- cgit v1.2.3 From 5e3c365cd4809def0c5b21136a1b5741a98ae35e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 22:56:24 +0100 Subject: rc: use sdlog2 -t option sdlog2: move all logs and conv.zip to "log" dir, messages cleanup --- ROMFS/px4fmu_common/init.d/rc.logging | 4 +- src/modules/sdlog2/sdlog2.c | 129 +++++++++++++++++----------------- 2 files changed, 68 insertions(+), 65 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055..6c02bf227 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -7,8 +7,8 @@ if [ -d /fs/microsd ] then if [ $BOARD == fmuv1 ] then - sdlog2 start -r 50 -a -b 16 + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e9d809834..656575573 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -114,7 +114,7 @@ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -211,12 +211,12 @@ static void handle_status(struct vehicle_status_s *cmd); /** * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logdir(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -298,67 +298,69 @@ int create_log_dir() time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - int n = snprintf(log_dir, sizeof(log_dir), "%s/", mountpoint); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0 && errno != EEXIST) { - warn("failed creating new dir"); + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); return -1; } } else { /* look for the next dir that does not exist */ while (dir_number <= MAX_NO_LOGFOLDER) { /* format log dir: e.g. /fs/microsd/sess001 */ - sprintf(log_dir, "%s/sess%03u", mountpoint, dir_number); + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret != 0) { - if (errno == EEXIST) { - /* dir exists already */ - dir_number++; - continue; - } else { - warn("failed creating new dir"); - return -1; - } + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; } - /* dir does not exist, success */ - break; + /* dir exists already */ + dir_number++; + continue; } if (dir_number >= MAX_NO_LOGFOLDER) { /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible dirs exist already.", MAX_NO_LOGFOLDER); + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } /* print logging path, important to find log file later */ - warnx("logging to dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] logging to dir: %s", log_dir); + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { /* string to hold the path to the log */ - char log_file[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; if (log_name_timestamp && gps_time != 0) { /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - int n = snprintf(log_file, sizeof(log_file), "%s/", log_dir); time_t gps_time_sec = gps_time / 1000000; struct tm t; gmtime_r(&gps_time_sec, &t); - n += strftime(log_file + n, sizeof(log_file) - n, "%H_%M_%S.bin", &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { uint16_t file_number = 1; // start with file log001 /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file, sizeof(log_file), "%s/log%03u.bin", log_dir, file_number); + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - if (!file_exist(log_file)) { + if (!file_exist(log_file_path)) { break; } file_number++; @@ -366,19 +368,19 @@ int open_logfile() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + warnx("all %d possible files exist already", MAX_NO_LOGFILE); return -1; } } - int fd = open(log_file, O_CREAT | O_WRONLY | O_DSYNC); + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] failed opening %s", log_file); + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("logging to: %s", log_file); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", log_file); + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -389,7 +391,7 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - int log_fd = open_logfile(); + int log_fd = open_log_file(); if (log_fd < 0) return; @@ -477,7 +479,7 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ @@ -486,16 +488,6 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } - const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", log_dir); - - if (file_copy(converter_in, converter_out)) { - warnx("unable to copy conversion scripts"); - } - - free(converter_out); - /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -524,7 +516,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -632,7 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ @@ -689,32 +681,43 @@ int sdlog2_thread_main(int argc, char *argv[]) case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } gps_time = 0; - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); + } + free(converter_out); + /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -935,7 +938,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -978,7 +981,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -1337,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1350,8 +1353,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1370,7 +1373,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1378,7 +1381,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1389,7 +1392,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } -- cgit v1.2.3 From ac37172b5280e661395035c17d2dfde91672b4b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:17:04 +0100 Subject: Hotfix: Prevent failures in boot handling due to missing microSD card logfile - we are not depending on the microSD any more completely --- ROMFS/px4fmu_common/init.d/rcS | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a9c5c59ea..a2e562d6e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,6 @@ # set MODE autostart -set LOG_FILE /fs/microsd/bootlog.txt set RC_FILE /fs/microsd/etc/rc.txt set CONFIG_FILE /fs/microsd/etc/config.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt @@ -21,10 +20,12 @@ set TUNE_OUT_ERROR ML< Date: Thu, 23 Jan 2014 08:25:56 +0100 Subject: Make startup less chatty --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a2e562d6e..cbbdeeee8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -181,7 +181,7 @@ then set IO_PRESENT yes else - echo "[init] PX4IO CRC failure, trying to update" + echo "[init] Trying to update" echo "PX4IO CRC failure" >> $LOG_FILE tone_alarm MLL32CP8MB -- cgit v1.2.3 From a0db455334c0928b953ab088e868e72fa3fc08f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 08:43:55 +0100 Subject: Cleanup wording in rcS --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cbbdeeee8..edf2c77fb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,7 +182,7 @@ then set IO_PRESENT yes else echo "[init] Trying to update" - echo "PX4IO CRC failure" >> $LOG_FILE + echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB -- cgit v1.2.3 From 1afe7f2c50016fbb39cb532252be171ba03ad357 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Jan 2014 18:39:32 +0100 Subject: Added tune on IO upgrade error --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index edf2c77fb..6f4e1f3b5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -199,10 +199,12 @@ then else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi else echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE + tone_alarm $TUNE_OUT_ERROR fi fi -- cgit v1.2.3 From ba90dc87f6fcb6198e36265239774d35a8efe9cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 16:43:44 +0100 Subject: HOTFIX: Re-enable legacy config support, uncomment commented out configs. Needs more work. --- ROMFS/px4fmu_common/init.d/rc.autostart | 40 ++++++++++++++++----------------- 1 file changed, 20 insertions(+), 20 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 325520dd0..75cac3e50 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -33,7 +33,7 @@ fi if param compare SYS_AUTOSTART 1002 then - #sh /etc/init.d/1002_rc_fw_state.hil + sh /etc/init.d/1002_rc_fw_state.hil fi if param compare SYS_AUTOSTART 1003 @@ -52,47 +52,47 @@ fi if param compare SYS_AUTOSTART 2100 100 then - #sh /etc/init.d/2100_mpx_easystar - #set MODE custom + sh /etc/init.d/2100_mpx_easystar + set MODE custom fi if param compare SYS_AUTOSTART 2101 101 then - #sh /etc/init.d/2101_hk_bixler - #set MODE custom + sh /etc/init.d/2101_hk_bixler + set MODE custom fi if param compare SYS_AUTOSTART 2102 102 then - #sh /etc/init.d/2102_3dr_skywalker - #set MODE custom + sh /etc/init.d/2102_3dr_skywalker + set MODE custom fi # # Flying wing # -if param compare SYS_AUTOSTART 3030 +if param compare SYS_AUTOSTART 3030 30 then - #sh /etc/init.d/3030_io_camflyer + sh /etc/init.d/3030_io_camflyer fi -if param compare SYS_AUTOSTART 3031 +if param compare SYS_AUTOSTART 3031 31 then sh /etc/init.d/3031_phantom fi -if param compare SYS_AUTOSTART 3032 +if param compare SYS_AUTOSTART 3032 32 then sh /etc/init.d/3032_skywalker_x5 fi -if param compare SYS_AUTOSTART 3033 +if param compare SYS_AUTOSTART 3033 33 then sh /etc/init.d/3033_wingwing fi -if param compare SYS_AUTOSTART 3034 +if param compare SYS_AUTOSTART 3034 34 then sh /etc/init.d/3034_fx79 fi @@ -101,27 +101,27 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 +if param compare SYS_AUTOSTART 4008 8 then #sh /etc/init.d/4008_ardrone fi -if param compare SYS_AUTOSTART 4009 +if param compare SYS_AUTOSTART 4009 9 then #sh /etc/init.d/4009_ardrone_flow fi -if param compare SYS_AUTOSTART 4010 +if param compare SYS_AUTOSTART 4010 10 then sh /etc/init.d/4010_dji_f330 fi -if param compare SYS_AUTOSTART 4011 +if param compare SYS_AUTOSTART 4011 11 then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 +if param compare SYS_AUTOSTART 4012 12 then sh /etc/init.d/4012_hk_x550 fi @@ -130,12 +130,12 @@ fi # Wide arm / H frame # -if param compare SYS_AUTOSTART 10015 +if param compare SYS_AUTOSTART 10015 15 then sh /etc/init.d/10015_tbs_discovery fi -if param compare SYS_AUTOSTART 10016 +if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi -- cgit v1.2.3 From 068cc190e20b8901b8e4128f3c21200724f0cd4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:18 +0100 Subject: Updated / added all system types that have been available before --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 58 +++------------ ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +++------------ .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 1 - ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 ++----------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +------------ ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 +++++++++------------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 33 ++++++++- ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 ++++++++++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 ++++++++++ 13 files changed, 312 insertions(+), 238 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm (limited to 'ROMFS/px4fmu_common') 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 40a13b5d1..ebe8a1a1e 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save fi -# Allow USB some time to come up -sleep 1 -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyACM0 - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 1 - -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT 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 75a00a675..46da24d35 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -43,4 +43,3 @@ set HIL yes set VEHICLE_TYPE fw set MIXER FMU_AERT - diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 000000000..5f3cec4e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba07..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace96..1ed923b19 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c974..cbcc6189b 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 03f282237..143310af9 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,8 +7,37 @@ if [ $DO_AUTOCONFIG == yes ] then - # TODO + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi set VEHICLE_TYPE fw -set MIXER FMU_Q +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 000000000..2e5f6ca4f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 000000000..ddec8f36e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 000000000..106e0fb54 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 000000000..f0eea339b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 000000000..992a7aeba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 -- cgit v1.2.3 From 4c4cd41b72471ce28ccebc4b24b592d6159f8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:14:31 +0100 Subject: Registered all new system types --- ROMFS/px4fmu_common/init.d/rc.autostart | 54 +++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 75cac3e50..34da2dfef 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -126,6 +126,51 @@ then sh /etc/init.d/4012_hk_x550 fi +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + # # Wide arm / H frame # @@ -139,3 +184,12 @@ if param compare SYS_AUTOSTART 10016 16 then sh /etc/init.d/10016_3dr_iris fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi -- cgit v1.2.3 From 7e855b5a23c9053dc8a59fcf33670e1cb93dd34b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jan 2014 17:20:24 +0100 Subject: Deleted test - should not be in mainline --- ROMFS/px4fmu_common/init.d/cmp_test | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85b..000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file -- cgit v1.2.3 From 9cdc13185b4a956c32a2281e637284a13bfd1a40 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 30 Jan 2014 23:09:20 +0100 Subject: Remove differential aileron mixing as this will result in a pitching effect on flying wings. --- ROMFS/px4fmu_common/mixers/FMU_Q.mix | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index 17ff71151..b8ecbc879 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 6000 6000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 -8000 -8000 0 -10000 10000 +S: 0 1 -6000 -6000 0 -10000 10000 Output 2 -------- -- cgit v1.2.3 From 091fe61bf22cdc38462b2b3aacf253d60125db6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 16:04:51 +0100 Subject: Comment in HIL start script --- ROMFS/px4fmu_common/init.d/rc.autostart | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'ROMFS/px4fmu_common') diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 34da2dfef..00baed646 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -23,7 +23,7 @@ if param compare SYS_AUTOSTART 1000 then - #sh /etc/init.d/1000_rc_fw_easystar.hil + sh /etc/init.d/1000_rc_fw_easystar.hil fi if param compare SYS_AUTOSTART 1001 -- cgit v1.2.3