diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_mag.h | 25 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 122 | ||||
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 131 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 498 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 87 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 57 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 12 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 33 | ||||
-rw-r--r-- | src/modules/systemlib/mavlink_log.c (renamed from src/modules/mavlink/mavlink_log.c) | 21 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 3 | ||||
-rw-r--r-- | src/systemcmds/config/config.c | 189 |
14 files changed, 716 insertions, 469 deletions
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 692f890bd..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 @@ -192,6 +192,11 @@ private: void stop(); /** + * Reset the device + */ + int reset(); + + /** * Perform the on-sensor scale calibration routine. * * @note The sensor will continue to provide measurements, these @@ -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,68 +547,67 @@ 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) 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 */ @@ -633,17 +637,24 @@ 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: - /* not supported, always 1 sample per poll */ - return -EINVAL; + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + + case MAGIOCGSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return 1000000/TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; @@ -697,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) { @@ -861,10 +879,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 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; @@ -211,6 +216,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. * @@ -274,6 +284,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 * * @return 0 on success, 1 on failure @@ -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; @@ -683,6 +685,14 @@ L3GD20::set_samplerate(unsigned frequency) } 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() { /* make sure we are stopped first */ @@ -702,6 +712,30 @@ L3GD20::stop() } 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) { L3GD20 *dev = (L3GD20 *)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 3e6ce64b8..efe7cf8eb 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,6 +54,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> +#include <systemlib/geo/geo.h> #include <nuttx/arch.h> #include <nuttx/clock.h> @@ -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 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 LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -180,56 +189,61 @@ 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_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_onchip_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 +254,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 +277,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 +316,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 +324,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 +335,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 +344,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,24 +353,25 @@ 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. + * 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 set_antialias_filter_bandwidth(unsigned bandwith); + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); /** - * Get the LSM303D accel anti-alias filter. + * Set the driver lowpass filter bandwidth. * * @param bandwidth The anti-alias filter bandwidth in Hz - * @return OK if the value was read and supported, ERROR otherwise. + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. */ - int get_antialias_filter_bandwidth(unsigned &bandwidth); + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); /** * Set the LSM303D internal accel sampling frequency. @@ -345,7 +381,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 +391,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,38 +432,43 @@ 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), - _mag_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(0), + _accel_onchip_filter_bandwith(0), _mag_range_ga(0.0f), + _mag_range_scale(0.0f), + _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(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; // 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 +519,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 +537,28 @@ 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(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(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + + _accel_read = 0; + _mag_read = 0; +} + int LSM303D::probe() { @@ -612,64 +660,55 @@ 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 */ - return ioctl(filp, SENSORIOCSPOLLRATE, 800); + return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { - /* 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; + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* adjust sample rate of sensor */ - set_samplerate(arg); + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; - // 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); + /* adjust filters */ + 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... */ - _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 +717,77 @@ 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; - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } 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: + /* arg needs to be in G */ + return accel_set_range(arg); + + case ACCELIOCGRANGE: + /* 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 */ 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 +818,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 +833,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 +880,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 +905,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 +923,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 +1005,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_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) { - new_range_g = 4.0f; + _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) { - new_range_g = 6.0f; + _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) { - new_range_g = 8.0f; + _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) { - new_range_g = 16.0f; + _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -953,8 +1043,7 @@ LSM303D::set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = new_range_g * 9.80665f; - _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); @@ -966,29 +1055,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 +1084,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 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::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; @@ -1015,15 +1102,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_onchip_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_onchip_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_onchip_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_onchip_filter_bandwith = 773; } else { return -EINVAL; @@ -1035,26 +1126,17 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) } int -LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) +LSM303D::accel_set_driver_lowpass_filter(float samplerate, float 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; + _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::set_samplerate(unsigned frequency) +LSM303D::accel_set_samplerate(unsigned frequency) { uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; @@ -1064,23 +1146,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 +1184,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 +1313,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 +1367,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 +1383,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 +1392,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 +1556,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 +1574,7 @@ test() /* XXX add poll-rate tests here too */ -// reset(); + reset(); errx(0, "PASS"); } @@ -1503,7 +1593,17 @@ 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"); + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + 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 pollrate reset failed"); + } exit(0); } 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)); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b1712e85e..21847f11b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -193,6 +193,11 @@ public: void print_status(); /** + * Disable RC input handling + */ + int disable_rc_handling(); + + /** * Set the DSM VCC is controlled by relay one flag * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled @@ -223,7 +228,8 @@ private: unsigned _max_relays; ///<Maximum relays supported by PX4IO unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO - unsigned _update_interval; ///<Subscription interval limiting send rate + unsigned _update_interval; ///< Subscription interval limiting send rate + bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values volatile int _task; ///<worker task id volatile bool _task_should_exit; ///<worker terminate flag @@ -294,6 +300,11 @@ private: int io_get_status(); /** + * Disable RC input handling + */ + int io_disable_rc_handling(); + + /** * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. @@ -411,6 +422,7 @@ PX4IO::PX4IO(device::Device *interface) : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), + _rc_handling_disabled(false), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -615,12 +627,16 @@ PX4IO::init() PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); - /* publish RC config to IO */ - ret = io_set_rc_config(); - if (ret != OK) { - log("failed to update RC input config"); - mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); - return ret; + if (_rc_handling_disabled) { + ret = io_disable_rc_handling(); + } else { + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); + return ret; + } } } @@ -916,6 +932,21 @@ PX4IO::io_set_arming_state() } int +PX4IO::disable_rc_handling() +{ + return io_disable_rc_handling(); +} + +int +PX4IO::io_disable_rc_handling() +{ + uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; + uint16_t clear = 0; + + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); +} + +int PX4IO::io_set_rc_config() { unsigned offset = 0; @@ -1456,7 +1487,7 @@ PX4IO::print_status() (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), @@ -1866,6 +1897,16 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + /* disable RC handling on request */ + if (argc > 0 && !strcmp(argv[0], "norc")) { + + if(g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); + + } else { + warnx("unknown argument: %s", argv[0]); + } + int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d38..5d3d6a73c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 38727c68c..deed25836 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -148,7 +148,8 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 97809d9c2..f5fa0ba75 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -74,7 +74,7 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PROTOCOL_VERSION 3 +#define PX4IO_PROTOCOL_VERSION 4 /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 @@ -159,6 +159,7 @@ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3ff9307cd..655a0c7a8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -158,9 +158,10 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -432,6 +433,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; // } + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + } + r_setup_arming = value; break; @@ -523,6 +528,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b237bd059..ded39c465 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -912,11 +912,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 @@ -929,6 +929,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -937,13 +938,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); - int ret = ioctl(fd, MAGIOCGEXTERNAL, 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"); + } + } + + + + 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/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 192195856..27608bdbf 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -46,28 +46,25 @@ #include <mavlink/mavlink_log.h> -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); - text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; } -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { return lb->count == 0; } -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { int end = (lb->start + lb->count) % lb->size; memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); @@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ } } -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { if (!mavlink_logbuffer_is_empty(lb)) { memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; - if (text_recorder_fd) { - fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); - fputc("\n", text_recorder_fd); - fsync(text_recorder_fd); - } - return 0; } else { @@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } } -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { va_list ap; va_start(ap, fmt); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c1227..cbf829122 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 5a02fd620..188dafa4e 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 <lm@inf.ethz.ch> + * Author: Julian Oes <joes@student.ethz.ch> * * 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 <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> * * 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); } } @@ -100,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -109,44 +100,45 @@ 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 */ + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + if (ret) + errx(ret,"sampling rate could not be set"); - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, i); + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - } else if (!strcmp(argv[0], "rate")) { + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + if (ret) + errx(ret,"pollrate could not be set"); - /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, i); - } + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); - } else if (argc > 0) { + if (ret) + errx(ret,"range could not be set"); - if(!strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); + } else if(argc == 1 && !strcmp(argv[0], "check")) { + 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); @@ -165,6 +157,7 @@ static void do_mag(int argc, char *argv[]) { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -174,31 +167,52 @@ do_mag(int argc, char *argv[]) } else { - if (argc > 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")) { - if (!strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 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) + 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:"); + 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 = 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); + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); close(fd); } @@ -210,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -219,50 +234,52 @@ 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 */ + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + if (ret) + errx(ret,"sampling rate could not be set"); - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, i); + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - } else if (!strcmp(argv[0], "rate")) { + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + if (ret) + errx(ret,"pollrate could not be set"); - /* 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 == 2 && !strcmp(argv[0], "range")) { + + /* 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")) { + 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); 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); } |