diff options
Diffstat (limited to 'ROMFS/px4fmu_common/init.d/10_io_f330')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/10_io_f330 | 43 |
1 files changed, 27 insertions, 16 deletions
diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 index 7b6509bf8..5af4f97b5 100644 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 10_io_f330: PX4FMU+PX4IO on a DJI F330 quad frame" +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" # # Load default params for this platform @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_P 0.3 - param save /fs/microsd/params + param save fi # @@ -32,24 +32,30 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink (depends on orb) -# -mavlink start -usleep 5000 -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io +set EXIT_ON_END no # -# Set PWM values for DJI ESCs +# Start and configure PX4IO or FMU interface # -px4io idle 900 900 900 900 -px4io min 1200 1200 1200 1200 -px4io max 1800 1800 1800 1800 +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 + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + set EXIT_ON_END yes +fi # # Load mixer @@ -65,3 +71,8 @@ pwm -u 400 -m 0xff # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi |