Merged master

4 files changed, 623 insertions, 221 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/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 65e8fa4b6..332348925 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -91,21 +91,61 @@
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
 class PX4IO : public device::I2C
 {
 public:
+	/**
+	 * Constructor.
+	 * 
+	 * Initialize all class variables.
+	 */
 	PX4IO();
+	/**
+	 * Destructor.
+	 * 
+	 * Wait for worker thread to terminate.
+	 */
 	virtual ~PX4IO();
 
+	/**
+	 * Initialize the PX4IO class.
+	 * 
+	 * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
 	virtual int		init();
 
+	/**
+	 * IO Control handler.
+	 * 
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * write handler.
+	 * 
+	 * Handle writes to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] buffer pointer to the data buffer to be written
+	 * @param[in] len size in bytes to be written
+	 * @return number of bytes written
+	 */
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate    The rate in Hz actuator outpus are sent to IO.
+	* @param[in] rate The rate in Hz actuator output are sent to IO.
 	*      Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
@@ -113,16 +153,16 @@ public:
 	/**
 	* Set the battery current scaling and bias
 	*
-	* @param amp_per_volt
-	* @param amp_bias
+	* @param[in] amp_per_volt
+	* @param[in] amp_bias
 	*/
 	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
 
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len Number of channels, could up to 8
+	 * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
@@ -142,15 +182,27 @@ 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();
 
+	/**
+	 * Set the DSM VCC is controlled by relay one flag
+	 *
+	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline void		set_dsm_vcc_ctl(bool enable)
 	{
 		_dsm_vcc_ctl = enable;
 	};
 
+	/**
+	 * Get the DSM VCC is controlled by relay one flag
+	 *
+	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline bool		get_dsm_vcc_ctl()
 	{
 		return _dsm_vcc_ctl;
@@ -158,59 +210,49 @@ public:
 
 private:
 	// XXX
-	unsigned		_max_actuators;
-	unsigned		_max_controls;
-	unsigned		_max_rc_input;
-	unsigned		_max_relays;
-	unsigned		_max_transfer;
+	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval; ///< subscription interval limiting send rate
+	unsigned 		_update_interval;	///<Subscription interval limiting send rate
 
-	volatile int		_task;		///< worker task
-	volatile bool		_task_should_exit;
+	volatile int		_task;			///<worker task id
+	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;
+	int			_mavlink_fd;		///<mavlink file descriptor
 
-	perf_counter_t		_perf_update;
+	perf_counter_t		_perf_update;		///<local performance counter
 
 	/* cached IO state */
-	uint16_t		_status;
-	uint16_t		_alarms;
+	uint16_t		_status;		///<Various IO status flags
+	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
+	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
+	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
-	actuator_controls_effective_s _controls_effective; ///< effective controls
+	actuator_outputs_s	_outputs;		///<mixed outputs
+	actuator_controls_effective_s _controls_effective; ///<effective controls
 
-	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_primary_pwm_device;	///<true if we are the default PWM output
 
-	float			_battery_amp_per_volt;
-	float			_battery_amp_bias;
-	float			_battery_mamphour_total;
-	uint64_t		_battery_last_timestamp;
+	float			_battery_amp_per_volt;	///<current sensor amps/volt
+	float			_battery_amp_bias;	///<current sensor bias
+	float			_battery_mamphour_total;///<amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
-	/**
-	 * Relay1 is dedicated to controlling DSM receiver power
-	 */
-
-	bool			_dsm_vcc_ctl;
-
-	/**
-	 * System armed
-	 */
-
-	bool			_system_armed;
+	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
 
 	/**
 	 * Trampoline to the worker task
@@ -380,8 +422,7 @@ PX4IO::PX4IO() :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false),
-	_system_armed(false)
+	_dsm_vcc_ctl(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -750,7 +791,7 @@ PX4IO::task_main()
 				// See if bind parameter has been set, and reset it to 0
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
 				if (dsm_bind_val) {
-					if (!_system_armed) {
+					if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
 							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
 							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -870,8 +911,6 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	_system_armed = armed.armed;
-
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1761,7 +1800,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
 {
 	unsigned count = len / 2;
 
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index b2c0db425..745cdfa40 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -48,156 +48,44 @@
 
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-
 #include "px4io.h"
 
-#define DSM_FRAME_SIZE		16
-#define DSM_FRAME_CHANNELS	7
-
-static int dsm_fd = -1;
-
-static hrt_abstime last_rx_time;
-static hrt_abstime last_frame_time;
-
-static uint8_t	frame[DSM_FRAME_SIZE];
-
-static unsigned partial_frame_count;
-static unsigned	channel_shift;
-
-unsigned dsm_frame_drops;
-
-static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-static void dsm_guess_format(bool reset);
-static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
-
-int
-dsm_init(const char *device)
-{
-	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+#define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
+#define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-	if (dsm_fd >= 0) {
-		struct termios t;
-
-		/* 115200bps, no parity, one stop bit */
-		tcgetattr(dsm_fd, &t);
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		tcsetattr(dsm_fd, TCSANOW, &t);
-
-		/* initialise the decoder */
-		partial_frame_count = 0;
-		last_rx_time = hrt_absolute_time();
-
-		/* reset the format detector */
-		dsm_guess_format(true);
-
-		debug("DSM: ready");
-
-	} else {
-		debug("DSM: open failed");
-	}
-
-	return dsm_fd;
-}
-
-void
-dsm_bind(uint16_t cmd, int pulses)
-{
-	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
-
-	if (dsm_fd < 0)
-		return;
-
-	switch (cmd) {
-	case dsm_bind_power_down:
-		// power down DSM satellite
-		POWER_RELAY1(0);
-		break;
-	case dsm_bind_power_up:
-		POWER_RELAY1(1);
-		dsm_guess_format(true);
-		break;
-	case dsm_bind_set_rx_out:
-		stm32_configgpio(usart1RxAsOutp); 
-		break;
-	case dsm_bind_send_pulses:
-		for (int i = 0; i < pulses; i++) {
-			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(25);
-			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
-		}
-		break;
-	case dsm_bind_reinit_uart:
-		// Restore USART rx pin
-		stm32_configgpio(GPIO_USART1_RX);
-		break;
-	}
-}
-
-bool
-dsm_input(uint16_t *values, uint16_t *num_values)
-{
-	ssize_t		ret;
-	hrt_abstime	now;
-
-	/*
-	 * The DSM* protocol doesn't provide any explicit framing,
-	 * so we detect frame boundaries by the inter-frame delay.
-	 *
-	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
-	 * frame transmission time is ~1.4ms.
-	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls,
-	 * the first byte we read will be the first byte of a frame.
-	 *
-	 * In the case where byte(s) are dropped from a frame, this also
-	 * provides a degree of protection. Of course, it would be better
-	 * if we didn't drop bytes...
-	 */
-	now = hrt_absolute_time();
-
-	if ((now - last_rx_time) > 5000) {
-		if (partial_frame_count > 0) {
-			dsm_frame_drops++;
-			partial_frame_count = 0;
-		}
-	}
-
-	/*
-	 * Fetch bytes, but no more than we would need to complete
-	 * the current frame.
-	 */
-	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
-
-	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
-		return false;
-
-	last_rx_time = now;
-
-	/*
-	 * Add bytes to the current frame
-	 */
-	partial_frame_count += ret;
-
-	/*
-	 * If we don't have a full frame, return
-	 */
-	if (partial_frame_count < DSM_FRAME_SIZE)
-		return false;
-
-	/*
-	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
-	 */
-	partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
-}
+static int dsm_fd = -1;				/**<File handle to the DSM UART*/
+static hrt_abstime dsm_last_rx_time;		/**<Timestamp when we last received*/
+static hrt_abstime dsm_last_frame_time;		/**<Timestamp for start of last dsm frame*/
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**<DSM dsm frame receive buffer*/
+static unsigned dsm_partial_frame_count;	/**<Count of bytes received for current dsm frame*/
+static unsigned dsm_channel_shift;		/**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
+static unsigned dsm_frame_drops;		/**<Count of incomplete DSM frames*/
 
+/**
+ * Attempt to decode a single channel raw channel datum
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ *
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ *
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm frame we attempt to decode it
+ *
+ * @param[in] raw 16 bit raw channel value from dsm frame
+ * @param[in] shift position of channel number in raw data
+ * @param[out] channel pointer to returned channel number
+ * @param[out] value pointer to returned channel value
+ * @return true=raw value successfully decoded
+ */
 static bool
 dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
@@ -215,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
 	return true;
 }
 
+/**
+ * Attempt to guess if receiving 10 or 11 bit channel values
+ *
+ * @param[in] reset true=reset the 10/11 bit state to unknown
+ */
 static void
 dsm_guess_format(bool reset)
 {
@@ -227,14 +120,14 @@ dsm_guess_format(bool reset)
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
-		channel_shift = 0;
+		dsm_channel_shift = 0;
 		return;
 	}
 
-	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
@@ -245,10 +138,10 @@ dsm_guess_format(bool reset)
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
 	}
 
-	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	/* wait until we have seen plenty of frames - 5 should normally be enough */
 	if (samples++ < 5)
 		return;
 
@@ -284,13 +177,13 @@ dsm_guess_format(bool reset)
 	}
 
 	if ((votes11 == 1) && (votes10 == 0)) {
-		channel_shift = 11;
+		dsm_channel_shift = 11;
 		debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
-		channel_shift = 10;
+		dsm_channel_shift = 10;
 		debug("DSM: 10-bit format");
 		return;
 	}
@@ -300,27 +193,131 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
+/**
+ * Initialize the DSM receive functionality
+ *
+ * Open the UART for receiving DSM frames and configure it appropriately
+ *
+ * @param[in] device Device name of DSM UART
+ */
+int
+dsm_init(const char *device)
+{
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
+
+	if (dsm_fd >= 0) {
+
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		dsm_partial_frame_count = 0;
+		dsm_last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+
+		debug("DSM: open failed");
+
+	}
+
+	return dsm_fd;
+}
+
+/**
+ * Handle DSM satellite receiver bind mode handler
+ *
+ * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
+ * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
+ */
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp =
+		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+
+	case dsm_bind_power_down:
+
+		/*power down DSM satellite*/
+		POWER_RELAY1(0);
+		break;
+
+	case dsm_bind_power_up:
+
+		/*power up DSM satellite*/
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+
+	case dsm_bind_set_rx_out:
+
+		/*Set UART RX pin to active output mode*/
+		stm32_configgpio(usart1RxAsOutp);
+		break;
+
+	case dsm_bind_send_pulses:
+
+		/*Pulse RX pin a number of times*/
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(25);
+		}
+		break;
+
+	case dsm_bind_reinit_uart:
+
+		/*Restore USART RX pin to RS232 receive mode*/
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+
+	}
+}
+
+/**
+ * Decode the entire dsm frame (all contained channels)
+ *
+ * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=DSM frame successfully decoded, false=no update
+ */
 static bool
 dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
-
 	/*
-		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
+		dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
 	*/
 	/*
 	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
-	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+	if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
 		dsm_guess_format(true);
 
-	/* we have received something we think is a frame */
-	last_frame_time = frame_time;
+	/* we have received something we think is a dsm_frame */
+	dsm_last_frame_time = frame_time;
 
-	/* if we don't know the frame format, update the guessing state machine */
-	if (channel_shift == 0) {
+	/* if we don't know the dsm_frame format, update the guessing state machine */
+	if (dsm_channel_shift == 0) {
 		dsm_guess_format(false);
 		return false;
 	}
@@ -332,17 +329,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
-	 * second frame in variants of the protocol where more than
+	 * second dsm_frame in variants of the protocol where more than
 	 * seven channels are being transmitted.
 	 */
 
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
 			continue;
 
 		/* ignore channels out of range */
@@ -354,7 +351,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			*num_values = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (channel_shift == 11)
+		if (dsm_channel_shift == 11)
 			value /= 2;
 
 		value += 998;
@@ -385,7 +382,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (channel_shift == 11)
+	if (dsm_channel_shift == 11)
 		*num_values |= 0x8000;
 
 	/*
@@ -393,3 +390,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 */
 	return true;
 }
+
+/**
+ * Called periodically to check for input data from the DSM UART
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm frame.
+ * In the case where byte(s) are dropped from a dsm frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ * 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
+ * @return true=decoded raw channel values updated, false=no update
+ */
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - dsm_last_rx_time) > 5000) {
+		if (dsm_partial_frame_count > 0) {
+			dsm_frame_drops++;
+			dsm_partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current dsm frame.
+	 */
+	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	dsm_last_rx_time = now;
+
+	/*
+	 * Add bytes to the current dsm frame
+	 */
+	dsm_partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full dsm frame, return
+	 */
+	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a dsm frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index a020f8704..587ca4e30 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -187,7 +187,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
-extern void     dsm_bind(uint16_t cmd, int pulses);
+extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
@@ -195,9 +195,9 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 extern volatile uint8_t debug_level;
 
 /* send a debug message to the console */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
+extern void	isr_debug(uint8_t level, const char *fmt, ...);
 
 #ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
+void		i2c_dump(void);
+void		i2c_reset(void);
 #endif