diff options
Diffstat (limited to 'ROMFS/px4fmu_common/init.d/15_io_tbs')
-rw-r--r-- | ROMFS/px4fmu_common/init.d/15_io_tbs | 67 |
1 files changed, 67 insertions, 0 deletions
diff --git a/ROMFS/px4fmu_common/init.d/15_io_tbs b/ROMFS/px4fmu_common/init.d/15_io_tbs new file mode 100644 index 000000000..b4f063e52 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_io_tbs @@ -0,0 +1,67 @@ +#!nsh + +echo "[init] 15_io_tbs: PX4FMU+PX4IO on a Team Blacksheep Discovery 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 + + 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 + param set MC_ATT_P 5.0 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# 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 PWM values for DJI ESCs +# +px4io idle 900 900 900 900 +px4io min 1180 1180 1180 1180 +px4io max 1800 1800 1800 1800 + +# +# 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 -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor |