diff options
Diffstat (limited to 'ROMFS/px4fmu_common/init.d')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/102_3dr_skywalker | 89 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/12-13_hex | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl | 4 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.sensors | 16 | ||||
-rw-r--r--[-rwxr-xr-x] | ROMFS/px4fmu_common/init.d/rcS | 53 |
5 files changed, 145 insertions, 19 deletions
diff --git a/ROMFS/px4fmu_common/init.d/102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker new file mode 100644 index 000000000..e5d21c321 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/102_3dr_skywalker @@ -0,0 +1,89 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + 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 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + 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_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 + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# 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 + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index f83f6cfd0..a7578bcaf 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -78,7 +78,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix # # Set PWM output frequency to 400 Hz # -pwm rate -c 123456 -r 400 +pwm rate -a -r 400 # # Set disarmed, min and max PWM signals diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 8e0914d63..40b2ee68b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -97,9 +97,9 @@ fi # if [ $MKBLCTRL_FRAME == x ] then - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix else - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix fi # diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f17b650bc..070a4e7e3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -19,12 +19,18 @@ fi if mpu6000 start then echo "using MPU6000" - set BOARD fmuv1 -else - echo "using L3GD20 and LSM303D" - l3gd20 start - lsm303d start +fi + +if l3gd20 start +then + echo "using L3GD20(H)" +fi + +if lsm303d start +then set BOARD fmuv2 +else + set BOARD fmuv1 fi # Start airspeed sensors diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f318d2134..59d8cbc3a 100755..100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,6 +8,8 @@ # set MODE autostart +set logfile /fs/microsd/bootlog.txt + # # Try to mount the microSD card. # @@ -163,26 +165,49 @@ then nshterm /dev/ttyACM0 & fi - # # Upgrade PX4IO firmware # - if px4io detect + + 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 start + then + echo "PX4IO OK" + echo "PX4IO OK" >> $logfile + fi + + if px4io checkcrc $io_file then - echo "PX4IO running, not upgrading" + echo "PX4IO CRC OK" + echo "PX4IO CRC OK" >> $logfile else - echo "Attempting to upgrade PX4IO" - if px4io update + echo "PX4IO CRC failure" + echo "PX4IO CRC failure" >> $logfile + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file then - if [ -d /fs/microsd ] + usleep 200000 + if px4io start then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + echo "PX4IO restart OK" + echo "PX4IO restart OK" >> $logfile + tone_alarm MSPAA + else + echo "PX4IO restart failed" + echo "PX4IO restart failed" >> $logfile + tone_alarm MNGGG + sh /etc/init.d/rc.error fi - - # Allow IO to safely kick back to app - usleep 200000 else - echo "No PX4IO to upgrade here" + echo "PX4IO update failed" + echo "PX4IO update failed" >> $logfile + tone_alarm MNGGG fi fi @@ -329,6 +354,12 @@ then sh /etc/init.d/101_hk_bixler set MODE custom fi + + if param compare SYS_AUTOSTART 102 + then + sh /etc/init.d/102_3dr_skywalker + set MODE custom + fi if param compare SYS_AUTOSTART 900 then |