Merge remote-tracking branch 'upstream/master' into mtecs

17 files changed, 132 insertions, 58 deletions

diff --git a/Debug/NuttX b/Debug/NuttX
index 3b95e96b2..d34e9f5b4 100644
--- a/Debug/NuttX
+++ b/Debug/NuttX
@@ -34,10 +34,10 @@ define _showheap
 	else
 		set $MM_ALLOC_BIT = 0x80000000
 	end
-	printf "HEAP %d  %p - %p\n", $index, g_heapstart[$index], g_heapend[$index]
+	printf "HEAP %d  %p - %p\n", $index, g_mmheap.mm_heapstart[$index], g_mmheap.mm_heapend[$index]
 	printf "ptr      size\n"
-	set $node = (char *)g_heapstart[$index] + sizeof(struct mm_allocnode_s)
-	while $node < g_heapend[$index]
+	set $node = (char *)g_mmheap.mm_heapstart[$index] + sizeof(struct mm_allocnode_s)
+	while $node < g_mmheap.mm_heapend[$index]
 		printf "  %p", $node
 		set $nodestruct = (struct mm_allocnode_s *)$node
 		printf "  %u", $nodestruct->size
@@ -47,7 +47,7 @@ define _showheap
 		else
 			set $used = $used + $nodestruct->size
 		end
-		if ($nodestruct->size > g_heapsize) || (($node + $nodestruct->size) > g_heapend[$index])
+		if ($nodestruct->size > g_mmheap.mm_heapsize) || (($node + $nodestruct->size) > g_mmheap.mm_heapend[$index])
 			printf "  (BAD SIZE)"
 		end
 		printf "\n"
@@ -59,7 +59,7 @@ define _showheap
 end
 
 define showheap
-	set $nheaps = sizeof(g_heapstart) / sizeof(g_heapstart[0])
+	set $nheaps = sizeof(g_mmheap.mm_heapstart) / sizeof(g_mmheap.mm_heapstart[0])
 	printf "Printing %d heaps\n", $nheaps
 	set $heapindex = (int)0
 	while $heapindex < $nheaps
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
index 093edd0e0..7cc21b99f 100644
--- a/Debug/Nuttx.py
+++ b/Debug/Nuttx.py
@@ -306,12 +306,15 @@ class NX_show_heap (gdb.Command):
 
 	def __init__(self):
 		super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
-		if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8:
-			self._allocflag = 0x80000000
-			self._allocnodesize = 8
-		else:
+                struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
+                preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
+                if preceding_size == 2:
 			self._allocflag = 0x8000
-			self._allocnodesize = 4
+                elif preceding_size == 4:
+			self._allocflag = 0x80000000
+                else:
+                        raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
+                self._allocnodesize = struct_mm_allocnode_s.sizeof
 
 	def _node_allocated(self, allocnode):
 		if allocnode['preceding'] & self._allocflag:
@@ -333,7 +336,8 @@ class NX_show_heap (gdb.Command):
 				state = ''
 			else:
 				state = '(free)'
-			print '  {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
+			print '  {} {} {}'.format(allocnode.address + self._allocnodesize,
+                                                  self._node_size(allocnode), state)
 			cursor += self._node_size(allocnode) / self._allocnodesize
 
 	def invoke(self, args, from_tty):
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index ea5cf8deb..492afcb4c 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -120,6 +120,8 @@ then
 	set EXIT_ON_END no
 	set MAV_TYPE none
 	set LOAD_DEFAULT_APPS yes
+	set GPS yes
+	set GPS_FAKE no
 	
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -437,9 +439,19 @@ then
 		echo "[init] Start logging"
 		sh /etc/init.d/rc.logging
 	fi
-
-	gps start
 	
+	if [ $GPS == yes ]
+	then
+		echo "[init] Start GPS"
+		if [ $GPS_FAKE == yes ]
+		then
+			echo "[init] Faking GPS"
+			gps start -f
+		else
+			gps start
+		fi		
+	fi
+
 	#
 	# Start up ARDrone Motor interface
 	#
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 7a93e513e..972573f9f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm);
 /** get the lockdown override flag to enable outputs in HIL */
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_IOC(_PWM_SERVO_BASE, 22)
 
+/** force safety switch off (to disable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+
 /*
  *
  *
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 147d7123a..b7981e9c4 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -148,6 +148,7 @@ enum {
 	TONE_BATTERY_WARNING_FAST_TUNE,
 	TONE_GPS_WARNING_TUNE,
 	TONE_ARMING_FAILURE_TUNE,
+	TONE_PARACHUTE_RELEASE_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a70ff6c5c..7258d5f9e 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -736,6 +736,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 		// these are no-ops, as no safety switch
 		break;
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index e318e206a..8458c2fdb 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -91,6 +91,8 @@
 
 #include "uploader.h"
 
+#include "modules/dataman/dataman.h"
+
 extern device::Device *PX4IO_i2c_interface() weak_function;
 extern device::Device *PX4IO_serial_interface() weak_function;
 
@@ -568,9 +570,15 @@ int
 PX4IO::init()
 {
 	int ret;
+	param_t sys_restart_param;
+	int sys_restart_val = DM_INIT_REASON_VOLATILE;
 
 	ASSERT(_task == -1);
 
+	sys_restart_param = param_find("SYS_RESTART_TYPE");
+	/* Indicate restart type is unknown */
+	param_set(sys_restart_param, &sys_restart_val);
+
 	/* do regular cdev init */
 	ret = CDev::init();
 
@@ -720,6 +728,11 @@ PX4IO::init()
 			/* keep waiting for state change for 2 s */
 		} while (!safety.armed);
 
+		/* Indicate restart type is in-flight */
+		sys_restart_val = DM_INIT_REASON_IN_FLIGHT;
+		param_set(sys_restart_param, &sys_restart_val);
+
+
 		/* regular boot, no in-air restart, init IO */
 
 	} else {
@@ -745,6 +758,10 @@ PX4IO::init()
 			}
 		}
 
+		/* Indicate restart type is power on */
+		sys_restart_val = DM_INIT_REASON_POWER_ON;
+		param_set(sys_restart_param, &sys_restart_val);
+
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
@@ -2129,6 +2146,10 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET_DISABLE_LOCKDOWN:
 		*(unsigned *)arg = _lockdown_override;
+
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+		/* force safety swith off */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
 	case DSM_BIND_START:
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 8ed0de58c..810f4aacc 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -335,6 +335,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
 	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
+	_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag";			// parachute release
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -346,6 +347,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
 	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
+	_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release";	// parachute release
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index fe6c9bfaa..0fd3c9e9e 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -199,15 +199,9 @@ int led_init()
 	}
 
 	/* the blue LED is only available on FMUv1 but not FMUv2 */
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-
-	if (ioctl(leds, LED_ON, LED_BLUE)) {
-		warnx("Blue LED: ioctl fail\n");
-		return ERROR;
-	}
-
-#endif
+	(void)ioctl(leds, LED_ON, LED_BLUE);
 
+	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
@@ -217,11 +211,7 @@ int led_init()
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
 	if (rgbleds == -1) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
-#else
-		warnx("No RGB LED found");
-#endif
+		warnx("No RGB LED found at " RGBLED_DEVICE_PATH);
 	}
 
 	return 0;
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index c132b0040..7505ba358 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -416,26 +416,26 @@ static int
 _restart(dm_reset_reason reason)
 {
 	unsigned char buffer[2];
-	int offset, result = 0;
+	int offset = 0, result = 0;
 
 	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
 
 	/* Loop through all of the data segments and delete those that are not persistent */
-	offset = 0;
-
 	while (1) {
 		size_t len;
 
 		/* Get data segment at current offset */
 		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
-			result = -1;
+			/* must be at eof */
 			break;
 		}
 
 		len = read(g_task_fd, buffer, sizeof(buffer));
 
-		if (len == 0)
+		if (len != sizeof(buffer)) {
+			/* must be at eof */
 			break;
+		}
 
 		/* check if segment contains data */
 		if (buffer[0]) {
@@ -443,12 +443,12 @@ _restart(dm_reset_reason reason)
 
 			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
 			if (reason == DM_INIT_REASON_POWER_ON) {
-				if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
+				if (buffer[1] > DM_PERSIST_POWER_ON_RESET) {
 					clear_entry = 1;
 				}
 
 			} else {
-				if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
+				if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) {
 					clear_entry = 1;
 				}
 			}
@@ -628,6 +628,23 @@ task_main(int argc, char *argv[])
 
 	fsync(g_task_fd);
 
+	/* see if we need to erase any items based on restart type */
+	int sys_restart_val;
+	if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
+		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
+			warnx("Power on restart");
+			_restart(DM_INIT_REASON_POWER_ON);
+		}
+		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+			warnx("In flight restart");
+			_restart(DM_INIT_REASON_IN_FLIGHT);
+		}
+		else
+			warnx("Unknown restart");
+	}
+	else
+		warnx("Unknown restart");
+
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */
 	/* worker thread is shutting down but still processing requests */
@@ -724,7 +741,7 @@ start(void)
 		return -1;
 	}
 
-	/* wait for the thread to actuall initialize */
+	/* wait for the thread to actually initialize */
 	sem_wait(&g_init_sema);
 	sem_destroy(&g_init_sema);
 
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 33c9fcd15..4382baeb5 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -75,7 +75,8 @@ extern "C" {
 	/* The reason for the last reset */
 	typedef enum {
 		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
-		DM_INIT_REASON_IN_FLIGHT	/* Data survives in-flight resets only */
+		DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
+		DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
 	} dm_reset_reason;
 
 	/* Maximum size in bytes of a single item instance */
@@ -100,7 +101,7 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
-	/* Retrieve from the data manager store */
+	/* Erase all items of this type */
 	__EXPORT int
 	dm_clear(
 		dm_item_t item			/* The item type to clear */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 6b0c44fb3..36d95bf06 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -496,7 +496,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt);
+			yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 65f4cbeaa..7c625a0c5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -148,17 +148,17 @@ private:
 		param_t xy_vel_d;
 		param_t xy_vel_max;
 		param_t xy_ff;
-		param_t tilt_max;
+		param_t tilt_max_air;
 		param_t land_speed;
-		param_t land_tilt_max;
+		param_t tilt_max_land;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
 		float thr_min;
 		float thr_max;
-		float tilt_max;
+		float tilt_max_air;
 		float land_speed;
-		float land_tilt_max;
+		float tilt_max_land;
 
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
@@ -308,9 +308,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.xy_vel_d	= param_find("MPC_XY_VEL_D");
 	_params_handles.xy_vel_max	= param_find("MPC_XY_VEL_MAX");
 	_params_handles.xy_ff		= param_find("MPC_XY_FF");
-	_params_handles.tilt_max	= param_find("MPC_TILT_MAX");
+	_params_handles.tilt_max_air	= param_find("MPC_TILTMAX_AIR");
 	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
-	_params_handles.land_tilt_max	= param_find("MPC_LAND_TILT");
+	_params_handles.tilt_max_land	= param_find("MPC_TILTMAX_LND");
 
 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -355,11 +355,11 @@ MulticopterPositionControl::parameters_update(bool force)
 	if (updated || force) {
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
-		param_get(_params_handles.tilt_max, &_params.tilt_max);
-		_params.tilt_max = math::radians(_params.tilt_max);
+		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
+		_params.tilt_max_air = math::radians(_params.tilt_max_air);
 		param_get(_params_handles.land_speed, &_params.land_speed);
-		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
-		_params.land_tilt_max = math::radians(_params.land_tilt_max);
+		param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
+		_params.tilt_max_land = math::radians(_params.tilt_max_land);
 
 		float v;
 		param_get(_params_handles.xy_p, &v);
@@ -841,13 +841,13 @@ MulticopterPositionControl::task_main()
 						thr_min = 0.0f;
 					}
 
-					float tilt_max = _params.tilt_max;
+					float tilt_max = _params.tilt_max_air;
 
 					/* adjust limits for landing mode */
 					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
 					    _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 						/* limit max tilt and min lift when landing */
-						tilt_max = _params.land_tilt_max;
+						tilt_max = _params.tilt_max_land;
 
 						if (thr_min < 0.0f) {
 							thr_min = 0.0f;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index a978d483a..6c20d6006 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -211,6 +211,10 @@ enum {							/* DSM bind states */
 						/* 12 occupied by CRC */
 #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF	       12	/* force safety switch into
+                                                           'armed' (PWM enabled) state */
+#define PX4IO_FORCE_SAFETY_MAGIC            22027       /* required argument for force safety (random) */
+
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
 #define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 6752e7b4b..9e5d7e7e2 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -570,6 +570,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
+		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			}
+			break;
+
 		default:
 			return -1;
 		}
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index ec2bc3a9a..702e435ac 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -62,12 +62,23 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
 PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
 
 /**
- * Set usage of IO board
- *
- * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
- *
- * @min 0
- * @max 1
- * @group System
- */
+* Set usage of IO board
+*
+* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
+*
+* @min 0
+* @max 1
+* @group System
+*/
 PARAM_DEFINE_INT32(SYS_USE_IO, 1);
+
+/**
+* Set restart type
+*
+* Set by px4io to indicate type of restart
+*
+* @min 0
+* @max 2
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index ad1996694..7d80af307 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -182,7 +182,7 @@ esc_calib_main(int argc, char *argv[])
 
 	if (orb_updated) {
 		errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
-			"\tmultirotor_att_control stop\n"
+			"\tmc_att_control stop\n"
 			"\tfw_att_control stop\n");
 	}