From dccdc977d5be4f957bcaea036b66d0391b29fd2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Aug 2013 17:56:32 +0200 Subject: [PATCH 1/5] Made accel / gyro self tests aware of offsets and scales, added support to config command to call these --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++- src/drivers/mpu6000/mpu6000.cpp | 69 ++++++++++++++++++- .../commander/accelerometer_calibration.c | 14 +++- src/systemcmds/config/config.c | 63 ++++++++++++++++- 4 files changed, 174 insertions(+), 7 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1ffca2f43a..744abfa006 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -260,6 +260,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 */ @@ -519,6 +526,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); @@ -713,7 +723,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); @@ -727,6 +738,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. */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c4e331a30e..ce8fe9feec 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/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,54 @@ 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; +} + +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 +898,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 +981,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 */ diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 6a304896a3..fbb73d997d 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/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++) { diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 2dad2261b5..42814f2b2f 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -129,7 +129,19 @@ 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."); + } 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 +160,41 @@ 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."); + } 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 +230,19 @@ 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."); + } 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"); } From dc1dc25f1bce95b1e4c8d26a418412416f7f8b48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 20:44:00 +0200 Subject: [PATCH 2/5] Revert "Hotfix for UART5" This reverts commit b08ca02410cc02f75bbfe154963edcea6767972e. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index ae941804ec..5b91930c91 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 From d90345a16619a6a056ca9158961db36787d97678 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 17 Aug 2013 21:48:54 -0700 Subject: [PATCH 3/5] Some early gdb/Python macros for working with NuttX. Note; only tested with gdb 7.6. Will definitely not work with the stock PX4 toolchain's gdb. --- Debug/Nuttx.py | 298 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 298 insertions(+) create mode 100644 Debug/Nuttx.py diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py new file mode 100644 index 0000000000..b0864a2293 --- /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 '{}()'.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 "".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() + + + + From e943488e9f85b7e8982bf137dbbe7f8183da21bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Aug 2013 10:07:37 +0200 Subject: [PATCH 4/5] Show values when selftest fails --- src/systemcmds/config/config.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 42814f2b2f..5a02fd6206 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -135,7 +135,11 @@ do_gyro(int argc, char *argv[]) int ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { - warnx("gyro self test FAILED! Check calibration."); + 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"); } @@ -177,6 +181,10 @@ do_mag(int argc, char *argv[]) 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"); } @@ -237,6 +245,10 @@ do_accel(int argc, char *argv[]) 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"); } From ae3a549d5780c58d54a9b54186fc309720bbfde6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 10:35:23 +0200 Subject: [PATCH 5/5] Fixed accel self test --- src/drivers/mpu6000/mpu6000.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce8fe9feec..4f70756006 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -689,6 +689,8 @@ MPU6000::accel_self_test() 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