Merge pull request #398 from NosDE/master

mkblctrl fix and qgroundcontrol2 startup script for different frametypes

ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl

@@ -0,0 +1,122 @@
+#!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
+	# 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
+	fmu mode_pwm
+	# 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
+
+#
+# Load mixer
+#
+if [ $MKBLCTRL_FRAME == x ]
+then
+    mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+else
+    mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+fi
+
+#
+# Set PWM output frequency
+#
+#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

ROMFS/px4fmu_common/init.d/rcS

@@ -182,6 +182,42 @@ then
 		sh /etc/init.d/16_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 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 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 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 20
+	then
+		set MKBLCTRL_MODE yes
+		set MKBLCTRL_FRAME +
+		sh /etc/init.d/rc.custom_dji_f330_mkblctrl
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 30
 	then

src/drivers/mkblctrl/mkblctrl.cpp

@@ -96,9 +96,10 @@ class MK : public device::I2C
 {
 public:
 	enum Mode {
+		MODE_NONE,
 		MODE_2PWM,
 		MODE_4PWM,
-		MODE_NONE
+		MODE_6PWM,
 	};
 
 	enum MappingMode {
@@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		return ret;
 
 	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
+	/*
 	switch (_mode) {
 	case MODE_2PWM:
 	case MODE_4PWM:
+	case MODE_6PWM:
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 		debug("not in a PWM mode");
 		break;
 	}
+	*/
+	ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
 	if (ret == -ENOTTY)