Merge branch 'master' of github.com:PX4/Firmware into fmuv2_bringup

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()
+
+
+
+

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

src/drivers/l3gd20/l3gd20.cpp

@@ -272,6 +272,13 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			set_samplerate(unsigned frequency);
+
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
 };
 
 /* helper macro for handling report buffer indices */
@@ -563,6 +570,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGRANGE:
 		return _current_range;
 
+	case GYROIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -791,7 +801,8 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+	if (_gyro_topic > 0)
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);
@@ -805,6 +816,28 @@ L3GD20::print_info()
 	       _num_reports, _oldest_report, _next_report, _reports);
 }
 
+int
+L3GD20::self_test()
+{
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
+}
+
 /**
  * Local functions in support of the shell command.
  */

src/drivers/mpu6000/mpu6000.cpp

@@ -285,12 +285,26 @@ private:
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
 	/**
-	 * Self test
+	 * Measurement self test
 	 *
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
 
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			gyro_self_test();
+
 	/*
 	  set low pass filter frequency
 	 */
@@ -321,6 +335,7 @@ protected:
 	void			parent_poll_notify();
 private:
 	MPU6000			*_parent;
+
 };
 
 /** driver 'main' command */
@@ -653,6 +668,56 @@ MPU6000::self_test()
 	return (_reads > 0) ? 0 : 1;
 }
 
+int
+MPU6000::accel_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* inspect accel offsets */
+	if (fabsf(_accel_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	if (fabsf(_accel_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+		return 1;
+
+	return 0;
+}
+
+int
+MPU6000::gyro_self_test()
+{
+	if (self_test())
+		return 1;
+
+	/* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+	if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+		return 1;
+
+	if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+		return 1;
+	if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+		return 1;
+
+	return 0;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -835,7 +900,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_range_m_s2;
 
 	case ACCELIOCSELFTEST:
-		return self_test();
+		return accel_self_test();
 
 	default:
 		/* give it to the superclass */
@@ -918,7 +983,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _gyro_range_rad_s;
 
 	case GYROIOCSELFTEST:
-		return self_test();
+		return gyro_self_test();
 
 	default:
 		/* give it to the superclass */

src/modules/commander/accelerometer_calibration.c

@@ -226,6 +226,12 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 		if (orient < 0)
 			return ERROR;
 
+		if (data_collected[orient]) {
+			sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, str);
+			continue;
+		}
+
 		sprintf(str, "meas started: %s", orientation_strs[orient]);
 		mavlink_log_info(mavlink_fd, str);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
@@ -380,6 +386,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
+	int errcount = 0;
+
 	while (count < samples_num) {
 		int poll_ret = poll(fds, 1, 1000);
 		if (poll_ret == 1) {
@@ -389,8 +397,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
 			count++;
 		} else {
-			return ERROR;
+			errcount++;
+			continue;
 		}
+
+		if (errcount > samples_num / 10)
+			return ERROR;
 	}
 
 	for (int i = 0; i < 3; i++) {

src/systemcmds/config/config.c

@@ -129,7 +129,23 @@ do_gyro(int argc, char *argv[])
 				ioctl(fd, GYROIOCSRANGE, i);
 			}
 
-		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+		} else if (argc > 0) {
+
+			if(!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("gyro self test FAILED! Check calibration:");
+					struct gyro_scale scale;
+					ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("gyro calibration and self test OK");
+				}
+			}
+
+		} else {
 			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
 		}
 
@@ -148,6 +164,45 @@ do_gyro(int argc, char *argv[])
 static void
 do_mag(int argc, char *argv[])
 {
+	int fd;
+
+	fd = open(MAG_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warn("%s", MAG_DEVICE_PATH);
+		errx(1, "FATAL: no magnetometer found");
+
+	} else {
+
+		if (argc > 0) {
+
+			if (!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("mag self test FAILED! Check calibration.");
+					struct mag_scale scale;
+					ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("mag calibration and self test OK");
+				}
+			}
+
+		} else {
+			warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+		}
+
+		int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
+		int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+
+		warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+
+		close(fd);
+	}
+
 	exit(0);
 }
 
@@ -183,7 +238,23 @@ do_accel(int argc, char *argv[])
 				/* set the range to i dps */
 				ioctl(fd, ACCELIOCSRANGE, i);
 			}
-		} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
+		} else if (argc > 0) {
+
+			if (!strcmp(argv[0], "check")) {
+				int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+				if (ret) {
+					warnx("accel self test FAILED! Check calibration.");
+					struct accel_scale scale;
+					ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+					warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+					warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+				} else {
+					warnx("accel calibration and self test OK");
+				}
+			}
+
+		} else {
 			warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
 		}