Merge branch 'master' into rgbled_fix

16 files changed, 392 insertions, 42 deletions

diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone
index 7dbbb8284..f6f09ac22 100644
--- a/ROMFS/px4fmu_common/init.d/08_ardrone
+++ b/ROMFS/px4fmu_common/init.d/08_ardrone
@@ -30,6 +30,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
+param set BAT_V_SCALING 0.008381
 
 #
 # Start MAVLink
diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
index 6333aae56..9b739f245 100644
--- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow
+++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow
@@ -30,6 +30,7 @@ fi
 # MAV_TYPE     2 = quadrotor
 #
 param set MAV_TYPE 2
+param set BAT_V_SCALING 0.008381
 
 #
 # Start MAVLink and MAVLink Onboard (PX4FLOW Sensor)
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
new file mode 100644
index 000000000..bbe3b7e28
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
@@ -0,0 +1,110 @@
+#!nsh
+#
+# USB HIL start
+#
+
+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/
+#
+param set MAV_TYPE 2
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# 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_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"
+
diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
new file mode 100644
index 000000000..51bc61c9e
--- /dev/null
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 040b5d594..c2a8c0c6a 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -84,7 +84,12 @@ then
 		param select /fs/microsd/params
 		if [ -f /fs/microsd/params ]
 		then
-			param load /fs/microsd/params
+			if param load /fs/microsd/params
+			then
+				echo "Parameters loaded"
+			else
+				echo "Parameter file corrupt - ignoring"
+			fi
 		fi
 	#fi
 	
@@ -106,8 +111,13 @@ then
 		sh /etc/init.d/1000_rc.hil
 		set MODE custom
 	else
-		# Try to get an USB console
-		nshterm /dev/ttyACM0 &
+		if param compare SYS_AUTOSTART 1001
+		then
+			sh /etc/init.d/1001_rc_quad.hil
+		else
+			# Try to get an USB console
+			nshterm /dev/ttyACM0 &
+		fi
 	fi
 	
 	#
@@ -172,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
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 818b64436..4d41d0d07 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -84,7 +84,7 @@
 
 /* Power switch controls ******************************************************/
 
-#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
 
 #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
 
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index 0ea95bded..ccd01edf5 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
-	/* spektrum power enable is active high - disable it by default */
-	/* XXX might not want to do this on warm restart? */
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+	/* spektrum power enable is active high - enable it by default */
 	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
 
 	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1bc3e97a4..d0de26a1a 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/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)
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 5fff1feac..f9fb9eea9 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -204,6 +204,7 @@ public:
 	 */
 	int			disable_rc_handling();
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	/**
 	 * Set the DSM VCC is controlled by relay one flag
 	 *
@@ -223,6 +224,9 @@ public:
 	{
 		return _dsm_vcc_ctl;
 	};
+#endif
+
+	inline uint16_t		system_status() const {return _status;}
 
 private:
 	device::Device		*_interface;
@@ -274,8 +278,9 @@ private:
 	float			_battery_mamphour_total;///<amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
-
+#endif
 
 	/**
 	 * Trampoline to the worker task
@@ -461,8 +466,11 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	,_dsm_vcc_ctl(false)
+#endif
+
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -855,7 +863,7 @@ PX4IO::task_main()
 
 				// See if bind parameter has been set, and reset it to -1
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
-				if (dsm_bind_val >= 0) {
+				if (dsm_bind_val > -1) {
 					dsm_bind_ioctl(dsm_bind_val);
 					dsm_bind_val = -1;
 					param_set(dsm_bind_param, &dsm_bind_val);
@@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 		/* 0: dsm2, 1:dsmx */
-		if ((dsmMode >= 0) && (dsmMode <= 1)) {
+		if ((dsmMode == 0) || (dsmMode == 1)) {
 			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
 			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
 		} else {
@@ -1638,11 +1646,19 @@ PX4IO::print_status()
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	printf("rates 0x%04x default %u alt %u\n",
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
+#endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
 	printf("controls");
 	for (unsigned i = 0; i < _max_controls; i++)
@@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	}
 
 	case GPIO_RESET: {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		uint32_t bits = (1 << _max_relays) - 1;
 		/* don't touch relay1 if it's controlling RX vcc */
 		if (_dsm_vcc_ctl)
 			bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 	}
 
 	case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+			break;
+		}
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
+			break;
+		}
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
 		if (*(uint32_t *)arg == _io_reg_get_error)
 			ret = -EIO;
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
@@ -1990,6 +2028,7 @@ start(int argc, char *argv[])
 		}
 	}
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	int dsm_vcc_ctl;
 
 	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1998,6 +2037,7 @@ start(int argc, char *argv[])
 			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
 		}
 	}
+#endif
 	exit(0);
 }
 
@@ -2037,8 +2077,10 @@ bind(int argc, char *argv[])
 	if (g_dev == nullptr)
 		errx(1, "px4io must be started first");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	if (!g_dev->get_dsm_vcc_ctl())
 		errx(1, "DSM bind feature not enabled");
+#endif
 
 	if (argc < 3)
 		errx(0, "needs argument, use dsm2 or dsmx");
@@ -2049,9 +2091,12 @@ bind(int argc, char *argv[])
 		pulses = DSMX_BIND_PULSES;
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+		errx(1, "system must not be armed");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-
+#endif
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
 	exit(0);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 82a0349c9..369e6da62 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -163,7 +163,9 @@ int do_gyro_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
+	mavlink_log_info(mavlink_fd, "offset calibration done.");
 
+#if 0
 	/*** --- SCALING --- ***/
 
 	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
@@ -241,9 +243,14 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	float gyro_scale = baseline_integral / gyro_integral;
-	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
+	
 	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+#else
+	float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
+#endif
+
+	
 
 
 	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
@@ -277,9 +284,6 @@ int do_gyro_calibration(int mavlink_fd)
 			warn("WARNING: auto-save of params to storage failed");
 		}
 
-		// char buf[50];
-		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
 		/* third beep by cal end routine */
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 206e27db5..433976656 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -243,28 +243,35 @@ dsm_init(const char *device)
 void
 dsm_bind(uint16_t cmd, int pulses)
 {
+#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
+	#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
+#else
 	const uint32_t usart1RxAsOutp =
 		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
 
 	if (dsm_fd < 0)
 		return;
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-	// XXX implement
-	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
-#else
 	switch (cmd) {
 
 	case dsm_bind_power_down:
 
 		/*power down DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(0);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(0);
+#endif
 		break;
 
 	case dsm_bind_power_up:
 
 		/*power up DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(1);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(1);
+#endif
 		dsm_guess_format(true);
 		break;
 
@@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (dsm_channel_shift == 11)
+	if (dsm_channel_shift == 11) {
+		/* Set the 11-bit data indicator */
 		*num_values |= 0x8000;
+	}
 
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
@@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  * Upon receiving a full dsm frame we attempt to decode it.
  *
  * @param[out] values pointer to per channel array of decoded values
- * @param[out] num_values pointer to number of raw channel values returned
+ * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index f5fa0ba75..0e2cd1689 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -165,11 +165,13 @@
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
 #define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
 #define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
 #define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
+#endif
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 11f4d053d..66c4ca906 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -100,7 +100,9 @@ extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
 #define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
 #define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+#endif
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9c95fd1c5..8cb21e54f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -145,7 +145,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
 #else
@@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-#ifdef POWER_RELAY1
 			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
-#endif
-#ifdef POWER_RELAY2
 			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
-#endif
-#ifdef POWER_ACC1
 			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
-#endif
-#ifdef POWER_ACC2
 			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
-#endif
 			break;
+#endif
 
 		case PX4IO_P_SETUP_VBATT_SCALE:
 			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 950af2eba..4d719e0e1 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
-
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
+#endif
 PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
@@ -174,7 +175,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 /* PX4IOAR: 0.00838095238 */
 /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
 #endif
 
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index c69de52b7..59cbcf5fb 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -48,6 +48,7 @@
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <errno.h>
+#include <semaphore.h>
 
 #include <sys/stat.h>
 
@@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s);
 /** parameter update topic handle */
 static orb_advert_t param_topic = -1;
 
+static sem_t param_sem = { .semcount = 1 };
+
 /** lock the parameter store */
 static void
 param_lock(void)
 {
-	/* XXX */
+	//do {} while (sem_wait(&param_sem) != 0);
 }
 
 /** unlock the parameter store */
 static void
 param_unlock(void)
 {
-	/* XXX */
+	//sem_post(&param_sem);
 }
 
 /** assert that the parameter store is locked */
@@ -519,6 +522,10 @@ param_save_default(void)
 	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
 
 	if (fd < 0) {
+		/* do another attempt in case the unlink call is not synced yet */
+		usleep(5000);
+		fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+
 		warn("opening '%s' for writing failed", param_get_default_file());
 		return fd;
 	}
@@ -528,7 +535,7 @@ param_save_default(void)
 
 	if (result != 0) {
 		warn("error exporting parameters to '%s'", param_get_default_file());
-		unlink(param_get_default_file());
+		(void)unlink(param_get_default_file());
 		return result;
 	}