Merged seatbelt_multirotor_new / master

10 files changed, 507 insertions, 112 deletions

diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
new file mode 100644
index 000000000..b0864a229
--- /dev/null
+++ b/Debug/Nuttx.py
@@ -0,0 +1,298 @@
+# GDB/Python functions for dealing with NuttX
+
+import gdb, gdb.types
+
+class NX_task(object):
+	"""Reference to a NuttX task and methods for introspecting it"""
+
+	def __init__(self, tcb_ptr):
+		self._tcb = tcb_ptr.dereference()
+		self._group = self._tcb['group'].dereference()
+		self.pid = tcb_ptr['pid']
+
+	@classmethod
+	def for_tcb(cls, tcb):
+		"""return a task with the given TCB pointer"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			pidhash_entry = pidhash_value[i]
+			if pidhash_entry['tcb'] == tcb:
+				return cls(pidhash_entry['tcb'])
+		return None
+
+	@classmethod
+	def for_pid(cls, pid):
+		"""return a task for the given PID"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			pidhash_entry = pidhash_value[i]
+			if pidhash_entry['pid'] == pid:
+				return cls(pidhash_entry['tcb'])
+		return None
+
+	@staticmethod
+	def pids():
+		"""return a list of all PIDs"""
+		pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
+		pidhash_value = pidhash_sym.value()
+		pidhash_type = pidhash_sym.type
+		result = []
+		for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
+			entry = pidhash_value[i]
+			pid = int(entry['pid'])
+			if pid is not -1:
+				result.append(pid)
+		return result
+
+	@staticmethod
+	def tasks():
+		"""return a list of all tasks"""
+		tasks = []
+		for pid in NX_task.pids():
+			tasks.append(NX_task.for_pid(pid))
+		return tasks
+
+	def _state_is(self, state):
+		"""tests the current state of the task against the passed-in state name"""
+		statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
+		if self._tcb['task_state'] == statenames[state]:
+			return True
+		return False
+
+	@property
+	def stack_used(self):
+		"""calculate the stack used by the thread"""
+		if 'stack_used' not in self.__dict__:
+			stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer())
+			if stack_base == 0:
+				self.__dict__['stack_used'] = 0
+			else:
+				stack_limit = self._tcb['adj_stack_size']
+				for offset in range(0, stack_limit):
+					if stack_base[offset] != 0xff:
+						break
+				self.__dict__['stack_used'] = stack_limit - offset
+		return self.__dict__['stack_used']
+
+	@property
+	def name(self):
+		"""return the task's name"""
+		return self._tcb['name'].string()
+
+	@property
+	def state(self):
+		"""return the name of the task's current state"""
+		statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
+		for name,value in statenames.iteritems():
+			if value == self._tcb['task_state']:
+				return name
+		return 'UNKNOWN'
+
+	@property
+	def waiting_for(self):
+		"""return a description of what the task is waiting for, if it is waiting"""
+		if self._state_is('TSTATE_WAIT_SEM'):
+			waitsem = self._tcb['waitsem'].dereference()
+			waitsem_holder = waitsem['holder']
+			holder = NX_task.for_tcb(waitsem_holder['htcb'])
+			if holder is not None:
+				return '{}({})'.format(waitsem.address, holder.name)
+			else:
+				return '{}(<bad holder>)'.format(waitsem.address)
+		if self._state_is('TSTATE_WAIT_SIG'):
+			return 'signal'
+		return None
+
+	@property
+	def is_waiting(self):
+		"""tests whether the task is waiting for something"""
+		if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'):
+			return True
+
+	@property
+	def is_runnable(self):
+		"""tests whether the task is runnable"""
+		if (self._state_is('TSTATE_TASK_PENDING') or 
+			self._state_is('TSTATE_TASK_READYTORUN') or 
+			self._state_is('TSTATE_TASK_RUNNING')):
+			return True
+		return False
+
+	@property
+	def file_descriptors(self):
+		"""return a dictionary of file descriptors and inode pointers"""
+		filelist = self._group['tg_filelist']
+		filearray = filelist['fl_files']
+		result = dict()
+		for i in range(filearray.type.range()[0],filearray.type.range()[1]):
+			inode = long(filearray[i]['f_inode'])
+			if inode != 0:
+				result[i] = inode
+		return result
+
+	@property
+	def registers(self):
+		if 'registers' not in self.__dict__:
+			registers = dict()
+			if self._state_is('TSTATE_TASK_RUNNING'):
+				# XXX need to fix this to handle interrupt context
+				registers['R0']         = long(gdb.parse_and_eval('$r0'))
+				registers['R1']         = long(gdb.parse_and_eval('$r1'))
+				registers['R2']         = long(gdb.parse_and_eval('$r2'))
+				registers['R3']         = long(gdb.parse_and_eval('$r3'))
+				registers['R4']         = long(gdb.parse_and_eval('$r4'))
+				registers['R5']         = long(gdb.parse_and_eval('$r5'))
+				registers['R6']         = long(gdb.parse_and_eval('$r6'))
+				registers['R7']         = long(gdb.parse_and_eval('$r7'))
+				registers['R8']         = long(gdb.parse_and_eval('$r8'))
+				registers['R9']         = long(gdb.parse_and_eval('$r9'))
+				registers['R10']        = long(gdb.parse_and_eval('$r10'))
+				registers['R11']        = long(gdb.parse_and_eval('$r11'))
+				registers['R12']        = long(gdb.parse_and_eval('$r12'))
+				registers['R13']        = long(gdb.parse_and_eval('$r13'))
+				registers['SP']         = long(gdb.parse_and_eval('$sp'))
+				registers['R14']        = long(gdb.parse_and_eval('$r14'))
+				registers['LR']         = long(gdb.parse_and_eval('$lr'))
+				registers['R15']        = long(gdb.parse_and_eval('$r15'))
+				registers['PC']         = long(gdb.parse_and_eval('$pc'))
+				registers['XPSR']       = long(gdb.parse_and_eval('$xpsr'))
+				# this would only be valid if we were in an interrupt
+				registers['EXC_RETURN'] = 0
+				# we should be able to get this...
+				registers['PRIMASK']    = 0
+			else:
+				context = self._tcb['xcp']
+				regs = context['regs']
+				registers['R0']         = long(regs[27])
+				registers['R1']         = long(regs[28])
+				registers['R2']         = long(regs[29])
+				registers['R3']         = long(regs[30])
+				registers['R4']         = long(regs[2])
+				registers['R5']         = long(regs[3])
+				registers['R6']         = long(regs[4])
+				registers['R7']         = long(regs[5])
+				registers['R8']         = long(regs[6])
+				registers['R9']         = long(regs[7])
+				registers['R10']        = long(regs[8])
+				registers['R11']        = long(regs[9])
+				registers['R12']        = long(regs[31])
+				registers['R13']        = long(regs[0])
+				registers['SP']         = long(regs[0])
+				registers['R14']        = long(regs[32])
+				registers['LR']         = long(regs[32])
+				registers['R15']        = long(regs[33])
+				registers['PC']         = long(regs[33])
+				registers['XPSR']       = long(regs[34])
+				registers['EXC_RETURN'] = long(regs[10])
+				registers['PRIMASK']    = long(regs[1])
+			self.__dict__['registers'] = registers
+		return self.__dict__['registers']
+
+	def __repr__(self):
+		return "<NX_task {}>".format(self.pid)
+
+	def __str__(self):
+		return "{}:{}".format(self.pid, self.name)
+
+	def __format__(self, format_spec):
+		return format_spec.format(
+			pid              = self.pid,
+			name             = self.name,
+			state            = self.state,
+			waiting_for      = self.waiting_for,
+			stack_used       = self.stack_used,
+			stack_limit      = self._tcb['adj_stack_size'],
+			file_descriptors = self.file_descriptors,
+			registers	 = self.registers
+			)
+
+class NX_show_task (gdb.Command):
+	"""(NuttX) prints information about a task"""
+
+	def __init__(self):
+		super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER)
+
+	def invoke(self, arg, from_tty):
+		t = NX_task.for_pid(int(arg))
+		if t is not None:
+			my_fmt = 'PID:{pid}  name:{name}  state:{state}\n'
+			my_fmt += '  stack used {stack_used} of {stack_limit}\n'
+			if t.is_waiting:
+				my_fmt += '  waiting for {waiting_for}\n'
+			my_fmt += '  open files: {file_descriptors}\n'
+			my_fmt += '  R0  {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n'
+			my_fmt += '  R4  {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n'
+			my_fmt += '  R8  {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
+			my_fmt += '  R12 {registers[PC]:#010x}\n'
+			my_fmt += '  SP  {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
+			print format(t, my_fmt)
+
+class NX_show_tasks (gdb.Command):
+	"""(NuttX) prints a list of tasks"""
+
+	def __init__(self):
+		super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER)
+
+	def invoke(self, args, from_tty):
+		tasks = NX_task.tasks()
+		for t in tasks:
+			print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
+
+NX_show_task()
+NX_show_tasks()
+
+class NX_show_heap (gdb.Command):
+	"""(NuttX) prints the heap"""
+
+	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:
+			self._allocflag = 0x8000
+			self._allocnodesize = 4
+
+	def _node_allocated(self, allocnode):
+		if allocnode['preceding'] & self._allocflag:
+			return True
+		return False
+
+	def _node_size(self, allocnode):
+		return allocnode['size'] & ~self._allocflag
+
+	def _print_allocations(self, region_start, region_end):
+		if region_start >= region_end:
+			print 'heap region {} corrupt'.format(hex(region_start))
+			return
+		nodecount = region_end - region_start
+		print 'heap {} - {}'.format(region_start, region_end)
+		cursor = 1
+		while cursor < nodecount:
+			allocnode = region_start[cursor]
+			if self._node_allocated(allocnode):
+				state = ''
+			else:
+				state = '(free)'
+			print '  {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
+			cursor += self._node_size(allocnode) / self._allocnodesize
+
+	def invoke(self, args, from_tty):
+		heap = gdb.lookup_global_symbol('g_mmheap').value()
+		nregions = heap['mm_nregions']
+		region_starts = heap['mm_heapstart']
+		region_ends = heap['mm_heapend']
+		print "{} heap(s)".format(nregions)
+		# walk the heaps
+		for i in range(0, nregions):
+			self._print_allocations(region_starts[i], region_ends[i])
+
+NX_show_heap()
+
+
+
+
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 861d59b4b..a7a6725c6 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RXDMA is not set
 # CONFIG_UART4_RXDMA is not set
 # CONFIG_UART5_RS485 is not set
-CONFIG_UART5_RXDMA=n
+CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_USART7_RXDMA is not set
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0f90db858..b1712e85e 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -186,8 +186,10 @@ public:
 	int 			set_idle_values(const uint16_t *vals, unsigned len);
 
 	/**
-	* Print the current status of IO
-	*/
+	 * Print IO status.
+	 *
+	 * Print all relevant IO status information
+	 */
 	void			print_status();
 
 	/**
@@ -235,16 +237,16 @@ private:
 	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;	///< actuator controls topic
+	int			_t_actuators;		///< actuator controls topic
 	int			_t_actuator_armed;	///< system armed control topic
-	int 			_t_vehicle_control_mode;	///< vehicle control mode topic
-	int			_t_param;		///<parameter update topic
+	int 			_t_vehicle_control_mode;///< vehicle control mode topic
+	int			_t_param;		///< parameter update topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
-	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
-	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
-	orb_advert_t		_to_battery;		///<battery status / voltage topic
+	orb_advert_t 		_to_input_rc;		///< rc inputs from io
+	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
+	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
+	orb_advert_t		_to_battery;		///< battery status / voltage
 	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 98aab8788..d2455794a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -399,16 +399,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_NAV_TAKEOFF: {
-			transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+			if (armed->armed) {
+				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 
-			if (nav_res == TRANSITION_CHANGED) {
-				mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
-			}
+				if (nav_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+				}
 
-			if (nav_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				if (nav_res != TRANSITION_DENIED) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
 			} else {
+				/* reject TAKEOFF not armed */
 				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index b2f6b33e3..0d5a537ea 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -211,14 +211,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
-	bool reset_sp_alt = true;
-	bool reset_sp_pos = true;
+	bool global_pos_sp_reproject = false;
+	bool global_pos_sp_valid = false;
+	bool local_pos_sp_valid = false;
+	bool reset_sp_z = true;
+	bool reset_sp_xy = true;
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+	bool reset_integral = true;
+
 	hrt_abstime t_prev = 0;
 	/* integrate in NED frame to estimate wind but not attitude offset */
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
-	float home_alt = 0.0f;
-	hrt_abstime home_alt_t = 0;
+	float ref_alt = 0.0f;
+	hrt_abstime ref_alt_t = 0;
 	uint64_t local_ref_timestamp = 0;
 
 	PID_t xy_pos_pids[2];
@@ -242,11 +251,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 
 	int paramcheck_counter = 0;
-	bool global_pos_sp_updated = false;
 
 	while (!thread_should_exit) {
-		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-
 		/* check parameters at 1 Hz */
 		if (++paramcheck_counter >= 50) {
 			paramcheck_counter = 0;
@@ -260,11 +266,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
 					/* use integral_limit_out = tilt_max / 2 */
 					float i_limit;
+
 					if (params.xy_vel_i == 0.0) {
 						i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+
 					} else {
 						i_limit = 1.0f;	// not used really
 					}
+
 					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
 				}
 
@@ -273,9 +282,20 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* only check global position setpoint updates but not read */
-		if (!global_pos_sp_updated) {
-			orb_check(global_pos_sp_sub, &global_pos_sp_updated);
+		bool updated;
+
+		orb_check(control_mode_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		}
+
+		orb_check(global_pos_sp_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+			global_pos_sp_valid = true;
+			global_pos_sp_reproject = true;
 		}
 
 		hrt_abstime t = hrt_absolute_time();
@@ -288,6 +308,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			dt = 0.0f;
 		}
 
+		if (control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = control_mode.flag_armed;
+
 		t_prev = t;
 
 		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
@@ -296,77 +326,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
 			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 
-			if (control_mode.flag_control_manual_enabled) {
-				/* manual mode, reset setpoints if needed */
-				if (reset_sp_alt) {
-					reset_sp_alt = false;
-					local_pos_sp.z = local_pos.z;
-					thrust_pid_set_integral(&z_vel_pid, -manual.throttle);	// thrust PID uses Z downside
-					mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
-				}
-
-				if (control_mode.flag_control_position_enabled && reset_sp_pos) {
-					reset_sp_pos = false;
-					local_pos_sp.x = local_pos.x;
-					local_pos_sp.y = local_pos.y;
-					pid_reset_integral(&xy_vel_pids[0]);
-					pid_reset_integral(&xy_vel_pids[1]);
-					mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y);
-				}
-			} else {
-				/* non-manual mode, project global setpoints to local frame */
-				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-				if (local_pos.ref_timestamp != local_ref_timestamp) {
-					local_ref_timestamp = local_pos.ref_timestamp;
-					/* init local projection using local position home */
-					double lat_home = local_pos.ref_lat * 1e-7;
-					double lon_home = local_pos.ref_lon * 1e-7;
-					map_projection_init(lat_home, lon_home);
-					warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
-					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
-				}
-
-				if (global_pos_sp_updated) {
-					global_pos_sp_updated = false;
-					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
-					double sp_lat = global_pos_sp.lat * 1e-7;
-					double sp_lon = global_pos_sp.lon * 1e-7;
-					/* project global setpoint to local setpoint */
-					map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
-
-					if (global_pos_sp.altitude_is_relative) {
-						local_pos_sp.z = -global_pos_sp.altitude;
-
-					} else {
-						local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
-					}
-
-					warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
-					mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
-					/* publish local position setpoint as projection of global position setpoint */
-					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
-				}
-			}
-
 			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
 			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
-
 			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
 
-			/* manual control - move setpoints */
 			if (control_mode.flag_control_manual_enabled) {
-				if (local_pos.ref_timestamp != home_alt_t) {
-					if (home_alt_t != 0) {
+				/* manual control */
+				/* check for reference point updates and correct setpoint */
+				if (local_pos.ref_timestamp != ref_alt_t) {
+					if (ref_alt_t != 0) {
 						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.ref_alt - home_alt;
+						local_pos_sp.z += local_pos.ref_alt - ref_alt;
 					}
 
-					home_alt_t = local_pos.ref_timestamp;
-					home_alt = local_pos.ref_alt;
+					ref_alt_t = local_pos.ref_timestamp;
+					ref_alt = local_pos.ref_alt;
+					// TODO also correct XY setpoint
 				}
 
+				/* reset setpoints to current position if needed */
 				if (control_mode.flag_control_altitude_enabled) {
-					/* move altitude setpoint with manual controls */
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z);
+					}
+
+					/* move altitude setpoint with throttle stick */
 					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
 
 					if (z_sp_ctl != 0.0f) {
@@ -383,7 +369,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				}
 
 				if (control_mode.flag_control_position_enabled) {
-					/* move position setpoint with manual controls */
+					if (reset_sp_xy) {
+						reset_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+					}
+
+					/* move position setpoint with roll/pitch stick */
 					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
 					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
 
@@ -410,12 +405,68 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				/* publish local position setpoint */
 				orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
+				/* local position setpoint is valid and can be used for loiter after position controlled mode */
+				local_pos_sp_valid = control_mode.flag_control_position_enabled;
+
+				/* force reprojection of global setpoint after manual mode */
+				global_pos_sp_reproject = true;
+
+			} else {
+				/* non-manual mode, use global setpoint */
+				/* init local projection using local position ref */
+				if (local_pos.ref_timestamp != local_ref_timestamp) {
+					global_pos_sp_reproject = true;
+					local_ref_timestamp = local_pos.ref_timestamp;
+					double lat_home = local_pos.ref_lat * 1e-7;
+					double lon_home = local_pos.ref_lon * 1e-7;
+					map_projection_init(lat_home, lon_home);
+					mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
+				}
+
+				if (global_pos_sp_reproject) {
+					/* update global setpoint projection */
+					global_pos_sp_reproject = false;
+
+					if (global_pos_sp_valid) {
+						/* global position setpoint valid, use it */
+						double sp_lat = global_pos_sp.lat * 1e-7;
+						double sp_lon = global_pos_sp.lon * 1e-7;
+						/* project global setpoint to local setpoint */
+						map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+						if (global_pos_sp.altitude_is_relative) {
+							local_pos_sp.z = -global_pos_sp.altitude;
+
+						} else {
+							local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+						}
+
+						mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+
+					} else {
+						if (!local_pos_sp_valid) {
+							/* local position setpoint is invalid,
+							 * use current position as setpoint for loiter */
+							local_pos_sp.x = local_pos.x;
+							local_pos_sp.y = local_pos.y;
+							local_pos_sp.z = local_pos.z;
+						}
+
+						mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+					}
+
+					/* publish local position setpoint as projection of global position setpoint */
+					orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+				}
 			}
 
 			/* run position & altitude controllers, calculate velocity setpoint */
 			if (control_mode.flag_control_altitude_enabled) {
 				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
+
 			} else {
+				reset_sp_z = true;
 				global_vel_sp.vz = 0.0f;
 			}
 
@@ -426,13 +477,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 				/* limit horizontal speed */
 				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+
 				if (xy_vel_sp_norm > 1.0f) {
 					global_vel_sp.vx /= xy_vel_sp_norm;
 					global_vel_sp.vy /= xy_vel_sp_norm;
 				}
 
 			} else {
-				reset_sp_pos = true;
+				reset_sp_xy = true;
 				global_vel_sp.vx = 0.0f;
 				global_vel_sp.vy = 0.0f;
 			}
@@ -444,20 +496,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
 				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
 				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
-				bool reset_integral = !control_mode.flag_armed;
+
 				if (control_mode.flag_control_climb_rate_enabled) {
-					if (reset_integral) {
-						thrust_pid_set_integral(&z_vel_pid, params.thr_min);
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = manual.throttle;
+
+							if (i < params.thr_min) {
+								i = params.thr_min;
+
+							} else if (i > params.thr_max) {
+								i = params.thr_max;
+							}
+						}
+
+						thrust_pid_set_integral(&z_vel_pid, -i);
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i);
 					}
+
 					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
 					att_sp.thrust = -thrust_sp[2];
+
+				} else {
+					/* reset thrust integral when altitude control enabled */
+					reset_int_z = true;
 				}
+
 				if (control_mode.flag_control_velocity_enabled) {
 					/* calculate thrust set point in NED frame */
-					if (reset_integral) {
+					if (reset_int_xy) {
+						reset_int_xy = false;
 						pid_reset_integral(&xy_vel_pids[0]);
 						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
 					}
+
 					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
 					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
 
@@ -471,11 +547,15 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (tilt > params.tilt_max) {
 						tilt = params.tilt_max;
 					}
+
 					/* convert direction to body frame */
 					thrust_xy_dir -= att.yaw;
 					/* calculate roll and pitch */
 					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
 					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+
+				} else {
+					reset_int_xy = true;
 				}
 
 				att_sp.timestamp = hrt_absolute_time();
@@ -483,14 +563,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				/* publish new attitude setpoint */
 				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			}
+
 		} else {
-			reset_sp_alt = true;
-			reset_sp_pos = true;
+			/* position controller disabled, reset setpoints */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+			global_pos_sp_reproject = true;
 		}
 
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
+
 		/* run at approximately 50 Hz */
 		usleep(20000);
-
 	}
 
 	warnx("stopped");
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 1094fd23b..0b09c9ea7 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +34,7 @@
 
 /*
  * @file multirotor_pos_control_params.c
- * 
+ *
  * Parameters for multirotor_pos_control
  */
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 14a3de2e5..3ec85a364 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,9 +33,9 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.h
- * 
- * Parameters for position controller
+ * @file multirotor_pos_control_params.h
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include <systemlib/param/param.h>
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81f938719..c0cfac880 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -508,6 +508,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (gps_valid) {
 				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
 				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+
 				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
 					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
 					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
@@ -554,6 +555,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			/* publish global position */
 			global_pos.valid = local_pos.global_xy;
+
 			if (local_pos.global_xy) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
@@ -561,21 +563,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.lon = (int32_t)(est_lon * 1e7);
 				global_pos.time_gps_usec = gps.time_gps_usec;
 			}
+
 			/* set valid values even if position is not valid */
 			if (local_pos.v_xy_valid) {
 				global_pos.vx = local_pos.vx;
 				global_pos.vy = local_pos.vy;
 				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
 			}
+
 			if (local_pos.z_valid) {
 				global_pos.relative_alt = -local_pos.z;
 			}
+
 			if (local_pos.global_z) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
+
 			if (local_pos.v_z_valid) {
 				global_pos.vz = local_pos.vz;
 			}
+
 			global_pos.timestamp = t;
 
 			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index dd98f65a9..d99892fe2 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -259,7 +259,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 67aeac372..4f4db5dbc 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -61,7 +61,7 @@
  */
 struct vehicle_control_mode_s
 {
-	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
+	uint16_t counter;   /**< incremented by the writing thread every time new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	bool flag_armed;
@@ -73,11 +73,9 @@ struct vehicle_control_mode_s
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
 	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
-	// XXX shouldn't be necessairy
-	//bool flag_auto_enabled;
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if horisontal speed (implies direction) is controlled */
+	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */