From d2d59aa39278de9461ff72061cbd8a89d7e81f4b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 13:04:57 +0200 Subject: Handle the config command line arguments a bit more intuitive --- src/systemcmds/config/config.c | 146 +++++++++++++++++------------------------ 1 file changed, 59 insertions(+), 87 deletions(-) (limited to 'src') diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 5a02fd620..766598ddd 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -2,6 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +36,7 @@ /** * @file config.c * @author Lorenz Meier + * @author Julian Oes * * config tool. */ @@ -69,27 +71,15 @@ config_main(int argc, char *argv[]) { if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { - if (argc >= 3) { - do_gyro(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_gyro(argc - 2, argv + 2); } if (!strcmp(argv[1], "accel")) { - if (argc >= 3) { - do_accel(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_accel(argc - 2, argv + 2); } if (!strcmp(argv[1], "mag")) { - if (argc >= 3) { - do_mag(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_mag(argc - 2, argv + 2); } } @@ -109,44 +99,36 @@ do_gyro(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the gyro internal sampling rate up to at least i Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { - - /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, i); - } + /* set the range to i dps */ + ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); - } else if (argc > 0) { + } else if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 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"); - } + 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"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\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"); } int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); @@ -174,29 +156,26 @@ do_mag(int argc, char *argv[]) } else { - if (argc > 0) { + if(argc == 1 && !strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 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"); - } + 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"); + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1;//ioctl(fd, MAGIOCGRANGE, 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); @@ -219,43 +198,36 @@ do_accel(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the accel internal sampling rate up to at least i Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + /* set the range to i m/s^2 */ + ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); - /* set the range to i dps */ - ioctl(fd, ACCELIOCSRANGE, i); - } - } 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 if(argc == 1 && !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"); + errx(1,"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 4 G\n\t"); } int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); -- cgit v1.2.3 From 307c9e52c775de2ce09ff4abf0bc1fb5db6dd41e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 19:50:58 +0200 Subject: Sorry, finally got the axes of the external mag right --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 692f890bd..44304fc22 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -861,10 +861,10 @@ HMC5883::collect() } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch and invert x and y */ + * therefore switch x and y and invert y */ _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD -- cgit v1.2.3 From f5c92314f16fde650ee6f2f4fa20b7c2680a4b00 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 20:02:06 +0200 Subject: Improved LSM303D driver, plus some fixes to the HMC5883 --- src/drivers/drv_mag.h | 25 ++- src/drivers/hmc5883/hmc5883.cpp | 5 + src/drivers/lsm303d/lsm303d.cpp | 482 +++++++++++++++++++++++----------------- src/modules/sensors/sensors.cpp | 25 ++- src/systemcmds/config/config.c | 71 ++++-- 5 files changed, 379 insertions(+), 229 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e95034e8e..3de5625fd 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag); /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) +/** return the mag internal sample rate in Hz */ +#define MAGIOCGSAMPLERATE _MAGIOC(1) + /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(1) +#define MAGIOCSLOWPASS _MAGIOC(2) + +/** return the mag internal lowpass filter in Hz */ +#define MAGIOCGLOWPASS _MAGIOC(3) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(2) +#define MAGIOCSSCALE _MAGIOC(4) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(3) +#define MAGIOCGSCALE _MAGIOC(5) /** set the measurement range to handle (at least) arg Gauss */ -#define MAGIOCSRANGE _MAGIOC(4) +#define MAGIOCSRANGE _MAGIOC(6) + +/** return the current mag measurement range in Gauss */ +#define MAGIOCGRANGE _MAGIOC(7) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(5) +#define MAGIOCCALIBRATE _MAGIOC(8) /** excite strap */ -#define MAGIOCEXSTRAP _MAGIOC(6) +#define MAGIOCEXSTRAP _MAGIOC(9) /** perform self test and report status */ -#define MAGIOCSELFTEST _MAGIOC(7) +#define MAGIOCSELFTEST _MAGIOC(10) /** determine if mag is external or onboard */ -#define MAGIOCGEXTERNAL _MAGIOC(8) +#define MAGIOCGEXTERNAL _MAGIOC(11) #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 44304fc22..1afc16a9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -637,13 +637,18 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: /* not supported, always 1 sample per poll */ return -EINVAL; case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e6ce64b8..8d6dc0672 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -180,56 +180,62 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _current_samplerate; - - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; + struct accel_scale _accel_scale; + unsigned _accel_range_g; + float _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_filter_bandwith; + struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; orb_advert_t _mag_topic; + unsigned _accel_read; + unsigned _mag_read; + perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; @@ -240,12 +246,19 @@ private: /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -256,24 +269,38 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); /** * Read a register from the LSM303D @@ -281,7 +308,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -289,7 +316,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -300,7 +327,7 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** * Set the LSM303D accel measurement range. @@ -309,7 +336,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int accel_set_range(unsigned max_g); /** * Set the LSM303D mag measurement range. @@ -318,7 +345,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int mag_set_range(unsigned max_g); + int mag_set_range(unsigned max_g); /** * Set the LSM303D accel anti-alias filter. @@ -327,15 +354,7 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned bandwith); - - /** - * Get the LSM303D accel anti-alias filter. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * @return OK if the value was read and supported, ERROR otherwise. - */ - int get_antialias_filter_bandwidth(unsigned &bandwidth); + int accel_set_antialias_filter_bandwidth(unsigned bandwith); /** * Set the LSM303D internal accel sampling frequency. @@ -345,7 +364,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int accel_set_samplerate(unsigned frequency); /** * Set the LSM303D internal mag sampling frequency. @@ -355,7 +374,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int mag_set_samplerate(unsigned frequency); + int mag_set_samplerate(unsigned frequency); }; /** @@ -396,16 +415,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_accel_report(0), _oldest_accel_report(0), _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), + _accel_range_g(8), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(800), + _accel_filter_bandwith(50), + _mag_range_ga(2), _mag_range_scale(0.0f), - _mag_range_ga(0.0f), + _mag_samplerate(100), + _accel_topic(-1), + _mag_topic(-1), + _accel_read(0), + _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_filter_x(800, 30), @@ -416,18 +441,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _accel_scale.x_offset = 0; + _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; + _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; + _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; - _mag_scale.x_offset = 0; + _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; + _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; + _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; } @@ -478,27 +503,12 @@ LSM303D::init() if (_mag_reports == nullptr) goto out; + reset(); + /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); - - /* enable mag, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - - /* XXX should we enable FIFO? */ - - set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ - set_samplerate(400); /* max sample rate */ - - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ - mag_set_samplerate(100); - - /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -511,6 +521,24 @@ out: return ret; } +void +LSM303D::reset() +{ + /* enable accel*/ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + + /* enable mag */ + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + accel_set_range(_accel_range_g); + accel_set_samplerate(_accel_samplerate); + accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + + mag_set_range(_mag_range_ga); + mag_set_samplerate(_mag_samplerate); +} + int LSM303D::probe() { @@ -612,64 +640,60 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_accel_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - /* XXX for vibration tests with 800 Hz */ + /* Use 800Hz as it is filtered in the driver as well*/ return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; - /* adjust sample rate of sensor */ - set_samplerate(arg); + /* adjust filters */ + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_call_accel_interval == 0) @@ -678,66 +702,78 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* account for sentinel in the ring */ + arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; - if (nullptr == buf) - return -ENOMEM; + if (nullptr == buf) + return -ENOMEM; - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _num_accel_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; case ACCELIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_accel_interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + return OK; + } case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - - case ACCELIOCSSCALE: - { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; - } + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; } + } + + case ACCELIOCSRANGE: + return accel_set_range(arg); + + case ACCELIOCGRANGE: + return _accel_range_g; case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; + case ACCELIOCSELFTEST: + return accel_self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -768,7 +804,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ + /* 100 Hz is max for mag */ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ @@ -783,9 +819,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - /* adjust sample rate of sensor */ - mag_set_samplerate(arg); - /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; @@ -833,17 +866,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return _num_mag_reports - 1; case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + reset(); + return OK; case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: @@ -857,17 +891,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; + return mag_self_test(); case MAGIOCGEXTERNAL: /* no external mag board yet */ @@ -879,6 +909,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::accel_self_test() +{ + if (_accel_read == 0) + 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 +LSM303D::mag_self_test() +{ + if (_mag_read == 0) + return 1; + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.y_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.z_offset) < 0.000001f) + return 1; + + return 0; +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -914,38 +991,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_g) +LSM303D::accel_set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range_g = 0.0f; float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range_g = 2.0f; + _accel_range_g = 2; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range_g = 4.0f; + _accel_range_g = 4; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range_g = 6.0f; + _accel_range_g = 6; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range_g = 8.0f; + _accel_range_g = 8; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range_g = 16.0f; + _accel_range_g = 16; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -953,7 +1029,7 @@ LSM303D::set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -966,29 +1042,28 @@ LSM303D::mag_set_range(unsigned max_ga) { uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_range = 0.0f; float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; if (max_ga <= 2) { - new_range = 2.0f; + _mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { - new_range = 4.0f; + _mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { - new_range = 8.0f; + _mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { - new_range = 12.0f; + _mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479e-3f; @@ -996,7 +1071,6 @@ LSM303D::mag_set_range(unsigned max_ga) return -EINVAL; } - _mag_range_ga = new_range; _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1005,7 +1079,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1015,15 +1089,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_filter_bandwith = 773; } else { return -EINVAL; @@ -1035,26 +1113,7 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) } int -LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) -{ - uint8_t readbits = read_reg(ADDR_CTRL_REG2); - - if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) - bandwidth = 50; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) - bandwidth = 194; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) - bandwidth = 362; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) - bandwidth = 773; - else - return ERROR; - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) +LSM303D::accel_set_samplerate(unsigned frequency) { uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; @@ -1064,23 +1123,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; - _current_samplerate = 100; + _accel_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; - _current_samplerate = 200; + _accel_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; - _current_samplerate = 400; + _accel_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; - _current_samplerate = 800; + _accel_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; - _current_samplerate = 1600; + _accel_samplerate = 1600; } else { return -EINVAL; @@ -1102,13 +1161,15 @@ LSM303D::mag_set_samplerate(unsigned frequency) if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; - + _mag_samplerate = 100; } else { return -EINVAL; @@ -1229,6 +1290,8 @@ LSM303D::measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + _accel_read++; + /* stop the perf counter */ perf_end(_accel_sample_perf); } @@ -1281,7 +1344,7 @@ LSM303D::mag_measure() mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report->scaling = _mag_range_scale; - mag_report->range_ga = _mag_range_ga; + mag_report->range_ga = (float)_mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); @@ -1297,6 +1360,8 @@ LSM303D::mag_measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + _mag_read++; + /* stop the perf counter */ perf_end(_mag_sample_perf); } @@ -1304,6 +1369,8 @@ LSM303D::mag_measure() void LSM303D::print_info() { + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); @@ -1466,7 +1533,7 @@ test() /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) errx(1, "failed to get if mag is onboard or external"); - warnx("device active: %s", ret ? "external" : "onboard"); + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); @@ -1484,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ -// reset(); + reset(); errx(0, "PASS"); } @@ -1503,7 +1570,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel driver poll rate reset failed"); + + int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) { + warnx("could not open to mag " MAG_DEVICE_PATH); + } else { + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "mag driver poll rate reset failed"); + } exit(0); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 198da9f0a..7ea1ae0f3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -936,6 +936,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -944,13 +945,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); +#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } +#endif + - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 766598ddd..262a52d20 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -90,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -102,20 +103,29 @@ do_gyro(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { warnx("gyro self test FAILED! Check calibration:"); @@ -147,6 +157,7 @@ static void do_mag(int argc, char *argv[]) { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -156,8 +167,32 @@ do_mag(int argc, char *argv[]) } else { - if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the mag internal sampling rate up to at least i Hz */ + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i G */ + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { warnx("mag self test FAILED! Check calibration:"); @@ -173,9 +208,9 @@ do_mag(int argc, char *argv[]) errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); + int range = 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); @@ -189,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -201,20 +237,29 @@ do_accel(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the range to i m/s^2 */ - ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + /* set the range to i G */ + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { warnx("accel self test FAILED! Check calibration:"); -- cgit v1.2.3 From 408b29ba618e1c2c7375d6acd488c223d796186f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 08:40:51 +0200 Subject: Don't store m/s^2 and G at the same time --- src/drivers/lsm303d/lsm303d.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8d6dc0672..2b7769992 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -220,7 +220,6 @@ private: struct accel_scale _accel_scale; unsigned _accel_range_g; - float _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -420,7 +419,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_g(8), - _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(800), _accel_filter_bandwith(50), @@ -1029,7 +1027,6 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1275,7 +1272,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + accel_report->range_m_s2 = _accel_range_g * 9.80665f; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); -- cgit v1.2.3 From 658276e1cc5f4639fb09ff41a20b422e6ce33fe3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:23:21 +0200 Subject: Add reset and samplerate ioctl to HMC5883 driver --- src/drivers/hmc5883/hmc5883.cpp | 107 ++++++++++++++++++++++------------------ 1 file changed, 60 insertions(+), 47 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 1afc16a9a..cb708db4a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -191,6 +191,11 @@ private: */ void stop(); + /** + * Reset the device + */ + int reset(); + /** * Perform the on-sensor scale calibration routine. * @@ -365,6 +370,9 @@ HMC5883::init() if (I2C::init() != OK) goto out; + /* reset the device configuration */ + reset(); + /* allocate basic report buffers */ _num_reports = 2; _reports = new struct mag_report[_num_reports]; @@ -381,9 +389,6 @@ HMC5883::init() if (_mag_topic < 0) debug("failed to create sensor_mag object"); - /* set range */ - set_range(_range_ga); - ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -542,62 +547,61 @@ int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) + return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -633,13 +637,15 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return reset(); case MAGIOCSSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + case MAGIOCGSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCGPOLLRATE, 0); case MAGIOCSRANGE: return set_range(arg); @@ -702,6 +708,13 @@ HMC5883::stop() work_cancel(HPWORK, &_work); } +int +HMC5883::reset() +{ + /* set range */ + return set_range(_range_ga); +} + void HMC5883::cycle_trampoline(void *arg) { -- cgit v1.2.3 From 9762cf86a081f44e7f7ea48f160d556003bf5be9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:52:21 +0200 Subject: Forgot to comment mag init in sensors.cpp back back in --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ea1ae0f3..dda558b4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -947,7 +947,7 @@ Sensors::mag_init() /* try different mag sampling rates */ -#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ @@ -962,7 +962,7 @@ Sensors::mag_init() errx(1, "FATAL: mag sampling rate could not be set"); } } -#endif + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); -- cgit v1.2.3 From 3875df2fe07d33d00331efd02394201d684655c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 10:44:47 +0200 Subject: Workaround to get the HMC5883 default rate right --- src/drivers/hmc5883/hmc5883.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index cb708db4a..d77f03bb7 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,8 +77,8 @@ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -/* Max measurement rate is 160Hz */ -#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ +/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ #define ADDR_CONF_A 0x00 #define ADDR_CONF_B 0x01 @@ -607,7 +607,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -645,7 +645,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return ioctl(filp, SENSORIOCGPOLLRATE, 0); + return 1000000/TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); -- cgit v1.2.3 From fc24037b03b0df3d8d75a98b929c7b37753e0337 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 12:37:06 +0200 Subject: Changed range handling of LSM303D once again, added defines for default values --- src/drivers/lsm303d/lsm303d.cpp | 63 +++++++++++++++++++++++++---------------- 1 file changed, 39 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2b7769992..803cd658f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -168,6 +169,14 @@ static const int ERROR = -1; #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 +/* default values for this device */ +#define ACCEL_DEFAULT_RANGE_G 8 +#define ACCEL_DEFAULT_SAMPLERATE 800 +#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MAG_DEFAULT_RANGE_GA 2 +#define MAG_DEFAULT_SAMPLERATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -219,7 +228,7 @@ private: struct mag_report *_mag_reports; struct accel_scale _accel_scale; - unsigned _accel_range_g; + unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_filter_bandwith; @@ -418,22 +427,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), - _accel_range_g(8), + _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), - _accel_samplerate(800), - _accel_filter_bandwith(50), - _mag_range_ga(2), + _accel_samplerate(0), + _accel_filter_bandwith(0), + _mag_range_ga(0.0f), _mag_range_scale(0.0f), - _mag_samplerate(100), + _mag_samplerate(0), _accel_topic(-1), _mag_topic(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(800, 30), - _accel_filter_y(800, 30), - _accel_filter_z(800, 30) + _accel_filter_x(0, 0), + _accel_filter_y(0, 0), + _accel_filter_z(0, 0) { // enable debug() calls _debug_enabled = true; @@ -529,12 +538,17 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - accel_set_range(_accel_range_g); - accel_set_samplerate(_accel_samplerate); - accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + + accel_set_range(ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); + accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); - mag_set_range(_mag_range_ga); - mag_set_samplerate(_mag_samplerate); + _accel_read = 0; + _mag_read = 0; } int @@ -658,8 +672,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* Use 800Hz as it is filtered in the driver as well*/ - return ioctl(filp, SENSORIOCSPOLLRATE, 800); + return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); /* adjust to a legal polling interval in Hz */ default: { @@ -759,10 +772,12 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) } case ACCELIOCSRANGE: + /* arg needs to be in G */ return accel_set_range(arg); case ACCELIOCGRANGE: - return _accel_range_g; + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -999,27 +1014,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_g = 2; + _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_g = 4; + _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_g = 6; + _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_g = 8; + _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_g = 16; + _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1027,7 +1042,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * 9.80665f; + _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1272,7 +1287,7 @@ LSM303D::measure() accel_report->z = _accel_filter_z.apply(z_in_new); accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_g * 9.80665f; + accel_report->range_m_s2 = _accel_range_m_s2; /* post a report to the ring - note, not locked */ INCREMENT(_next_accel_report, _num_accel_reports); -- cgit v1.2.3 From 7db420b9b222e6114e2cb6ffb5d726a946dd07c6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:20:42 +0200 Subject: Get units right in config --- src/systemcmds/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 262a52d20..188dafa4e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -212,7 +212,7 @@ do_mag(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = 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); + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); close(fd); } @@ -279,7 +279,7 @@ do_accel(int argc, char *argv[]) int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range); + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); close(fd); } -- cgit v1.2.3 From 8083efb60c97ffce5be8dcbf3956ab67cc17d729 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:11 +0200 Subject: Use gyro at correct rate --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index dda558b4c..2ffa5f698 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -919,11 +919,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif -- cgit v1.2.3 From 1ede95d252f5401f3bcf94265c42a060833ca8ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:54 +0200 Subject: L3GD20 and LSM303D reset and range config working properly now --- src/drivers/l3gd20/l3gd20.cpp | 131 ++++++++++++++++++++++++++-------------- src/drivers/lsm303d/lsm303d.cpp | 92 ++++++++++++++++------------ 2 files changed, 136 insertions(+), 87 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de6b753f1..5e0a2119a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -154,6 +154,10 @@ static const int ERROR = -1; #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) +#define L3GD20_DEFAULT_RATE 760 +#define L3GD20_DEFAULT_RANGE_DPS 2000 +#define L3GD20_DEFAULT_FILTER_FREQ 30 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -191,9 +195,10 @@ private: orb_advert_t _gyro_topic; unsigned _current_rate; - unsigned _current_range; unsigned _orientation; + unsigned _read; + perf_counter_t _sample_perf; math::LowPassFilter2p _gyro_filter_x; @@ -210,6 +215,11 @@ private: */ void stop(); + /** + * Reset the driver + */ + void reset(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -273,6 +283,14 @@ private: */ int set_samplerate(unsigned frequency); + /** + * Set the lowpass filter of the driver + * + * @param samplerate The current samplerate + * @param frequency The cutoff frequency for the lowpass filter + */ + void set_driver_lowpass_filter(float samplerate, float bandwidth); + /** * Self test * @@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_rad_s(0.0f), _gyro_topic(-1), _current_rate(0), - _current_range(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), - _gyro_filter_x(250, 30), - _gyro_filter_y(250, 30), - _gyro_filter_z(250, 30) + _read(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -349,22 +367,7 @@ L3GD20::init() memset(&_reports[0], 0, sizeof(_reports[0])); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - write_reg(ADDR_CTRL_REG4, REG4_BDU); - write_reg(ADDR_CTRL_REG5, 0); - - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - - /* disable FIFO. This makes things simpler and ensures we - * aren't getting stale data. It means we must run the hrt - * callback fast enough to not miss data. */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - - set_range(2000); /* default to 2000dps */ - set_samplerate(0); /* max sample rate */ + reset(); ret = OK; out: @@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; - // adjust filters - float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* adjust filters */ + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ if (want_start) @@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _num_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg); @@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + + return OK; + } case GYROIOCGLOWPASS: - return _gyro_filter_x.get_cutoff_freq(); + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: + /* arg should be in dps */ return set_range(arg); case GYROIOCGRANGE: - return _current_range; + /* convert to dps and round */ + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return self_test(); @@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; float new_range_scale_dps_digit; + float new_range; if (max_dps == 0) { max_dps = 2000; } if (max_dps <= 250) { - _current_range = 250; + new_range = 250; bits |= RANGE_250DPS; new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { - _current_range = 500; + new_range = 500; bits |= RANGE_500DPS; new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { - _current_range = 2000; + new_range = 2000; bits |= RANGE_2000DPS; new_range_scale_dps_digit = 70e-3f; @@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps) return -EINVAL; } - _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; + _gyro_range_rad_s = new_range / 180.0f * M_PI_F; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); @@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - // use limits good for H or non-H models + /* use limits good for H or non-H models */ if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; @@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency) return OK; } +void +L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth); +} + void L3GD20::start() { @@ -701,6 +711,30 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::reset() +{ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG4, REG4_BDU); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); + + set_samplerate(L3GD20_DEFAULT_RATE); + set_range(L3GD20_DEFAULT_RANGE_DPS); + set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); + + _read = 0; +} + void L3GD20::measure_trampoline(void *arg) { @@ -804,6 +838,8 @@ L3GD20::measure() if (_gyro_topic > 0) orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + _read++; + /* stop the perf counter */ perf_end(_sample_perf); } @@ -811,6 +847,7 @@ L3GD20::measure() void L3GD20::print_info() { + printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); @@ -949,7 +986,7 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel pollrate reset failed"); exit(0); } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 803cd658f..efe7cf8eb 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -170,13 +170,13 @@ static const int ERROR = -1; #define INT_SRC_M 0x13 /* default values for this device */ -#define ACCEL_DEFAULT_RANGE_G 8 -#define ACCEL_DEFAULT_SAMPLERATE 800 -#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 -#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MAG_DEFAULT_RANGE_GA 2 -#define MAG_DEFAULT_SAMPLERATE 100 +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -231,7 +231,7 @@ private: unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; - unsigned _accel_filter_bandwith; + unsigned _accel_onchip_filter_bandwith; struct mag_scale _mag_scale; unsigned _mag_range_ga; @@ -356,13 +356,22 @@ private: int mag_set_range(unsigned max_g); /** - * Set the LSM303D accel anti-alias filter. + * Set the LSM303D on-chip anti-alias filter bandwith. * * @param bandwidth The anti-alias filter bandwidth in Hz * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int accel_set_antialias_filter_bandwidth(unsigned bandwith); + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); + + /** + * Set the driver lowpass filter bandwidth. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), - _accel_filter_bandwith(0), + _accel_onchip_filter_bandwith(0), _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), @@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _accel_filter_x(0, 0), - _accel_filter_y(0, 0), - _accel_filter_z(0, 0) + _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -538,14 +547,13 @@ LSM303D::reset() write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - _accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - - accel_set_range(ACCEL_DEFAULT_RANGE_G); - accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE); - accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - mag_set_range(MAG_DEFAULT_RANGE_GA); - mag_set_samplerate(MAG_DEFAULT_SAMPLERATE); + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); _accel_read = 0; _mag_read = 0; @@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE); + return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* adjust filters */ - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - - _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); - return OK; + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); } case ACCELIOCGLOWPASS: @@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; - _accel_filter_bandwith = 50; + _accel_onchip_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; - _accel_filter_bandwith = 194; + _accel_onchip_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; - _accel_filter_bandwith = 362; + _accel_onchip_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; - _accel_filter_bandwith = 773; + _accel_onchip_filter_bandwith = 773; } else { return -EINVAL; @@ -1124,6 +1125,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) return OK; } +int +LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); + + return OK; +} + int LSM303D::accel_set_samplerate(unsigned frequency) { @@ -1582,15 +1593,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "accel driver poll rate reset failed"); + err(1, "accel pollrate reset failed"); - int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(MAG_DEVICE_PATH, O_RDONLY); - if (fd_mag < 0) { - warnx("could not open to mag " MAG_DEVICE_PATH); + if (fd < 0) { + warnx("mag could not be opened, external mag might be used"); } else { + /* no need to reset the mag as well, the reset() is the same */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "mag driver poll rate reset failed"); + err(1, "mag pollrate reset failed"); } exit(0); -- cgit v1.2.3 From 4f51f333a9a125ce137abe689c3fc0ce7943701b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:52:20 +0200 Subject: Adapted the MPU6000 to have the same get range ioctls and defines for defaults --- src/drivers/mpu6000/mpu6000.cpp | 87 +++++++++++++++++++++++------------------ 1 file changed, 48 insertions(+), 39 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bfc74c73e..0e65923db 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -149,6 +149,15 @@ #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A +#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_GYRO_DEFAULT_RANGE_G 8 +#define MPU6000_GYRO_DEFAULT_RATE 1000 +#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 class MPU6000_gyro; @@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _reads(0), _sample_rate(1000), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), - _accel_filter_x(1000, 30), - _accel_filter_y(1000, 30), - _accel_filter_z(1000, 30), - _gyro_filter_x(1000, 30), - _gyro_filter_y(1000, 30), - _gyro_filter_z(1000, 30) + _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) { // disable debug() calls _debug_enabled = false; @@ -485,14 +494,13 @@ void MPU6000::reset() up_udelay(1000); // SAMPLE RATE - //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(_sample_rate); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - _set_dlpf_filter(42); + _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -535,8 +543,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (9.81f / 4096.0f); - _accel_range_m_s2 = 8.0f * 9.81f; + _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; usleep(1000); @@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + case SENSOR_POLLRATE_DEFAULT: - /* set to same as sample rate per default */ - return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); + return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // XXX decide on relationship of both filters // i.e. disable the on-chip filter //_set_dlpf_filter((uint16_t)arg); - _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); - _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return _accel_range_m_s2; + return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -920,28 +929,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; - return -ENOMEM; - } + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + GyroReportBuffer *buf = new GyroReportBuffer(arg); + + if (nullptr == buf) + return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete _gyro_reports; + _gyro_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_rad_s = xx return -EINVAL; case GYROIOCGRANGE: - return _gyro_range_rad_s; + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: return gyro_self_test(); @@ -1400,7 +1409,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); -- cgit v1.2.3