diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/boards/px4fmuv2/module.mk | 5 | ||||
-rw-r--r-- | src/drivers/boards/px4fmuv2/px4fmu2_init.c (renamed from src/drivers/boards/px4fmuv2/px4fmu_init.c) | 5 | ||||
-rw-r--r-- | src/drivers/boards/px4fmuv2/px4fmu2_led.c | 85 | ||||
-rw-r--r-- | src/drivers/led/led.cpp | 4 | ||||
-rw-r--r-- | src/drivers/lsm303d/iirFilter.c | 255 | ||||
-rw-r--r-- | src/drivers/lsm303d/iirFilter.h | 59 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 120 | ||||
-rw-r--r-- | src/drivers/lsm303d/module.mk | 3 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 148 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 36 |
10 files changed, 579 insertions, 141 deletions
diff --git a/src/drivers/boards/px4fmuv2/module.mk b/src/drivers/boards/px4fmuv2/module.mk index eb8841e4d..99d37eeca 100644 --- a/src/drivers/boards/px4fmuv2/module.mk +++ b/src/drivers/boards/px4fmuv2/module.mk @@ -3,7 +3,8 @@ # SRCS = px4fmu_can.c \ - px4fmu_init.c \ + px4fmu2_init.c \ px4fmu_pwm_servo.c \ px4fmu_spi.c \ - px4fmu_usb.c + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmuv2/px4fmu_init.c b/src/drivers/boards/px4fmuv2/px4fmu2_init.c index 57f2812b8..535e075b2 100644 --- a/src/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/src/drivers/boards/px4fmuv2/px4fmu2_init.c @@ -192,9 +192,8 @@ __EXPORT int nsh_archinitialize(void) (hrt_callout)stm32_serial_dma_poll, NULL); - // initial LED state - // XXX need to make this work again -// drv_led_start(); + /* initial LED state */ + //drv_led_start(); up_ledoff(LED_AMBER); /* Configure SPI-based devices */ diff --git a/src/drivers/boards/px4fmuv2/px4fmu2_led.c b/src/drivers/boards/px4fmuv2/px4fmu2_led.c new file mode 100644 index 000000000..85177df56 --- /dev/null +++ b/src/drivers/boards/px4fmuv2/px4fmu2_led.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> + +#include "stm32.h" +#include "px4fmu_internal.h" + +#include <arch/board/board.h> + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 04b565358..27e43b245 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -110,7 +110,7 @@ LED *gLED; } void -drv_led_start() +drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; diff --git a/src/drivers/lsm303d/iirFilter.c b/src/drivers/lsm303d/iirFilter.c new file mode 100644 index 000000000..8311f14d6 --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.c @@ -0,0 +1,255 @@ +#include "math.h" +#include "string.h" +#include "iirFilter.h" + +/////////////////////////////////////////////////////////////////////////////// +// Internal function prototypes + +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, double Ts, TF_ZPG_t *pZpgd); + +int btDifcToZpgd(const TF_DIF_t *pkDifc, double Ts, TF_ZPG_t *pZpgd); + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt); + +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly); + +/////////////////////////////////////////////////////////////////////////////// +// external functions + +int testFunction() +{ + printf("TEST\n"); + return 1; +} + +int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt) +{ + TF_POLY_t polyd; + TF_ZPG_t zpgd; + + memset(pFilt, 0, sizeof(FIL_T)); + + // perform bilinear transform with frequency pre warping + btDifcToZpgd(pDifc, Ts, &zpgd); + + // calculate polynom + tZpgxToPolyx(&zpgd, &polyd); + + // set the filter parameters + tPolydToFil(&polyd, pFilt); + + return 1; +} + +// run filter using inp, return output of the filter +float updateFilter(FIL_T *pFilt, float inp) +{ + float outp = 0; + int idx; // index used for different things + int i; // loop variable + + // Store the input to the input array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->inpData[idx] = inp; + + // calculate numData * inpData + for (i = 0; i < pFilt->numLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->numLength + 1) % MAX_LENGTH; + outp += pFilt->numData[i] * pFilt->inpData[idx]; + } + + // calculate denData * outData + for (i = 0; i < pFilt->denLength; i++) + { + // index of inp array + idx = (pFilt->inpCnt + i - pFilt->denLength) % MAX_LENGTH; + outp -= pFilt->denData[i] * pFilt->outData[idx]; + } + + // store the ouput data to the output array + idx = pFilt->inpCnt % MAX_LENGTH; + pFilt->outData[idx] = outp; + + pFilt->inpCnt += 1; + + return outp; +} + +/////////////////////////////////////////////////////////////////////////////// +// Internal functions + +int tPolydToFil(const TF_POLY_t *pkPolyd, FIL_T *pFilt) +{ + double gain; + int i; + + if (pkPolyd->Ts < 0) + { + return 0; + } + + // initialize filter to 0 + memset(pFilt, 0, sizeof(FIL_T)); + + gain = pkPolyd->denData[pkPolyd->denLength - 1]; + pFilt->Ts = pkPolyd->Ts; + + pFilt->denLength = pkPolyd->denLength - 1; + pFilt->numLength = pkPolyd->numLength; + + for (i = 0; i < pkPolyd->numLength; i++) + { + pFilt->numData[i] = pkPolyd->numData[i] / gain; + } + + for (i = 0; i < (pkPolyd->denLength - 1); i++) + { + pFilt->denData[i] = pkPolyd->denData[i] / gain; + } +} + +// bilinear transformation of poles and zeros +int btDifcToZpgd(const TF_DIF_t *pkDifc, + double Ts, + TF_ZPG_t *pZpgd) +{ + TF_ZPG_t zpgc; + int totDiff; + int i; + + zpgc.zerosLength = 0; + zpgc.polesLength = 0; + zpgc.gain = pkDifc->gain; + zpgc.Ts = pkDifc->Ts; + + // Total number of differentiators / integerators + // positive diff, negative integ, 0 for no int/diff + totDiff = pkDifc->numDiff - pkDifc->numInt + pkDifc->highpassLength; + + while (zpgc.zerosLength < totDiff) + { + zpgc.zerosData[zpgc.zerosLength] = 0; + zpgc.zerosLength++; + } + while (zpgc.polesLength < -totDiff) + { + zpgc.polesData[zpgc.polesLength] = 0; + zpgc.polesLength++; + } + + // The next step is to calculate the poles + // This has to be done for the highpass and lowpass filters + // As we are using bilinear transformation there will be frequency + // warping which has to be accounted for + for (i = 0; i < pkDifc->lowpassLength; i++) + { + // pre-warping: + double freq = 2.0 / Ts * tan(pkDifc->lowpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // adjust gain such that lp has gain = 1 for low frequencies + zpgc.gain *= freq; + zpgc.polesLength++; + } + for (i = 0; i < pkDifc->highpassLength; i++) + { + // pre-warping + double freq = 2.0 / Ts * tan(pkDifc->highpassData[i] * 2.0 * M_PI * Ts / 2.0); + // calculate pole: + zpgc.polesData[zpgc.polesLength] = -freq; + // gain does not have to be adjusted + zpgc.polesLength++; + } + + return btZpgcToZpgd(&zpgc, Ts, pZpgd); +} + +// bilinear transformation of poles and zeros +int btZpgcToZpgd(const TF_ZPG_t *pkZpgc, + double Ts, + TF_ZPG_t *pZpgd) +{ + int i; + + // init digital gain + pZpgd->gain = pkZpgc->gain; + + // transform the poles + pZpgd->polesLength = pkZpgc->polesLength; + for (i = 0; i < pkZpgc->polesLength; i++) + { + pZpgd->polesData[i] = (2.0 / Ts + pkZpgc->polesData[i]) / (2.0 / Ts - pkZpgc->polesData[i]); + pZpgd->gain /= (2.0 / Ts - pkZpgc->polesData[i]); + } + + // transform the zeros + pZpgd->zerosLength = pkZpgc->zerosLength; + for (i = 0; i < pkZpgc->zerosLength; i++) + { + pZpgd->zerosData[i] = (2.0 / Ts + pkZpgc->zerosData[i]) / (2.0 / Ts + pkZpgc->zerosData[i]); + pZpgd->gain *= (2.0 / Ts - pkZpgc->zerosData[i]); + } + + // if we don't have the same number of poles as zeros we have to add + // poles or zeros due to the bilinear transformation + while (pZpgd->zerosLength < pZpgd->polesLength) + { + pZpgd->zerosData[pZpgd->zerosLength] = -1.0; + pZpgd->zerosLength++; + } + while (pZpgd->zerosLength > pZpgd->polesLength) + { + pZpgd->polesData[pZpgd->polesLength] = -1.0; + pZpgd->polesLength++; + } + + pZpgd->Ts = Ts; + + return 1; +} + +// calculate polynom from zero, pole, gain form +int tZpgxToPolyx(const TF_ZPG_t *pkZpg, TF_POLY_t *pPoly) +{ + int i, j; // counter variable + double tmp0, tmp1, gain; + + // copy Ts + pPoly->Ts = pkZpg->Ts; + + // calculate denominator polynom + pPoly->denLength = 1; + pPoly->denData[0] = 1.0; + for (i = 0; i < pkZpg->polesLength; i++) + { + // init temporary variable + tmp0 = 0.0; + // increase the polynom by 1 + pPoly->denData[pPoly->denLength] = 0; + pPoly->denLength++; + for (j = 0; j < pPoly->denLength; j++) + { + tmp1 = pPoly->denData[j]; + pPoly->denData[j] = tmp1 * -pkZpg->polesData[i] + tmp0; + tmp0 = tmp1; + } + } + + // calculate numerator polynom + pPoly->numLength = 1; + pPoly->numData[0] = pkZpg->gain; + for (i = 0; i < pkZpg->zerosLength; i++) + { + tmp0 = 0.0; + pPoly->numData[pPoly->numLength] = 0; + pPoly->numLength++; + for (j = 0; j < pPoly->numLength; j++) + { + tmp1 = pPoly->numData[j]; + pPoly->numData[j] = tmp1 * -pkZpg->zerosData[i] + tmp0; + tmp0 = tmp1; + } + } +} diff --git a/src/drivers/lsm303d/iirFilter.h b/src/drivers/lsm303d/iirFilter.h new file mode 100644 index 000000000..ab4223a8e --- /dev/null +++ b/src/drivers/lsm303d/iirFilter.h @@ -0,0 +1,59 @@ +#ifndef IIRFILTER__H__ +#define IIRFILTER__H__ + +__BEGIN_DECLS + +#define MAX_LENGTH 4 + +typedef struct FILTER_s +{ + float denData[MAX_LENGTH]; + float numData[MAX_LENGTH]; + int denLength; + int numLength; + float Ts; + float inpData[MAX_LENGTH]; + float outData[MAX_LENGTH]; + unsigned int inpCnt; +} FIL_T; + +typedef struct TF_ZPG_s +{ + int zerosLength; + double zerosData[MAX_LENGTH + 1]; + int polesLength; + double polesData[MAX_LENGTH + 1]; + double gain; + double Ts; +} TF_ZPG_t; + +typedef struct TF_POLY_s +{ + int numLength; + double numData[MAX_LENGTH]; + int denLength; + double denData[MAX_LENGTH]; + double Ts; +} TF_POLY_t; + +typedef struct TF_DIF_s +{ + int numInt; + int numDiff; + int lowpassLength; + int highpassLength; + double lowpassData[MAX_LENGTH]; + int highpassData[MAX_LENGTH]; + double gain; + double Ts; +} TF_DIF_t; + +__EXPORT int testFunction(void); + +__EXPORT float updateFilter(FIL_T *pFilt, float inp); + +__EXPORT int initFilter(const TF_DIF_t *pDifc, double Ts, FIL_T *pFilt); + +__END_DECLS + +#endif diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ba7316e55..844cc3884 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -65,6 +65,7 @@ #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include "iirFilter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -124,7 +125,7 @@ static const int ERROR = -1; #define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_CONT_UPDATE_A (0<<3) +#define REG1_BDU_UPDATE (1<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) @@ -156,11 +157,11 @@ static const int ERROR = -1; #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) -#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6)) -#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6)) -#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6)) -#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6)) -#define REG6_FULL_SCALE_12GA_M ((1<<7) | (1<<6)) +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) @@ -218,6 +219,12 @@ private: float _accel_range_m_s2; orb_advert_t _accel_topic; + unsigned _current_samplerate; + +// FIL_T _filter_x; +// FIL_T _filter_y; +// FIL_T _filter_z; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; @@ -393,6 +400,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _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), @@ -405,8 +413,6 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : // enable debug() calls _debug_enabled = true; - _mag_range_scale = 12.0f/32767.0f; - // default scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -476,7 +482,7 @@ LSM303D::init() _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); + 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); @@ -488,6 +494,22 @@ LSM303D::init() set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ + /* initiate filter */ +// TF_DIF_t tf_filter; +// tf_filter.numInt = 0; +// tf_filter.numDiff = 0; +// tf_filter.lowpassLength = 2; +// tf_filter.highpassLength = 0; +// tf_filter.lowpassData[0] = 10; +// tf_filter.lowpassData[1] = 10; +// //tf_filter.highpassData[0] = ; +// tf_filter.gain = 1; +// tf_filter.Ts = 1/_current_samplerate; +// +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_x); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_y); +// initFilter(&tf_filter, 1.0/(double)_current_samplerate, &_filter_z); + mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); @@ -705,6 +727,23 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) else return -EINVAL; + 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 ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; default: /* give it to the superclass */ @@ -856,37 +895,43 @@ LSM303D::set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range = 0.0f; + 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 = 2.0f; + new_range_g = 2.0f; setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range = 4.0f; + new_range_g = 4.0f; setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range = 6.0f; + new_range_g = 6.0f; setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range = 8.0f; + new_range_g = 8.0f; setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range = 16.0f; + new_range_g = 16.0f; setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; } else { return -EINVAL; } - _accel_range_m_s2 = new_range * 9.80665f; - _accel_range_scale = _accel_range_m_s2 / 32768.0f; + _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -899,6 +944,7 @@ 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; @@ -906,25 +952,29 @@ LSM303D::mag_set_range(unsigned max_ga) if (max_ga <= 2) { new_range = 2.0f; setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { new_range = 4.0f; setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { new_range = 8.0f; setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { new_range = 12.0f; setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; } else { return -EINVAL; } _mag_range_ga = new_range; - _mag_range_scale = _mag_range_ga / 32768.0f; + _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -991,18 +1041,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; + _current_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; + _current_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; + _current_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; + _current_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; + _current_samplerate = 1600; } else { return -EINVAL; @@ -1127,9 +1182,18 @@ LSM303D::measure() accel_report->y_raw = raw_accel_report.y; accel_report->z_raw = raw_accel_report.z; +// float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; +// float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; +// float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; +// +// accel_report->x = updateFilter(&_filter_x, x_in_new); +// accel_report->y = updateFilter(&_filter_y, y_in_new); +// accel_report->z = updateFilter(&_filter_z, z_in_new); + accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + accel_report->scaling = _accel_range_scale; accel_report->range_m_s2 = _accel_range_m_s2; @@ -1341,7 +1405,7 @@ void test() { int fd_accel = -1; - struct accel_report a_report; + struct accel_report accel_report; ssize_t sz; int filter_bandwidth; @@ -1352,20 +1416,20 @@ test() err(1, "%s open failed", ACCEL_DEVICE_PATH); /* do a simple demand read */ - sz = read(fd_accel, &a_report, sizeof(a_report)); + sz = read(fd_accel, &accel_report, sizeof(accel_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(accel_report)) err(1, "immediate read failed"); - warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x); - warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y); - warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z); - warnx("accel x: \t%d\traw", (int)a_report.x_raw); - warnx("accel y: \t%d\traw", (int)a_report.y_raw); - warnx("accel z: \t%d\traw", (int)a_report.z_raw); + warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); + warnx("accel x: \t%d\traw", (int)accel_report.x_raw); + warnx("accel y: \t%d\traw", (int)accel_report.y_raw); + warnx("accel z: \t%d\traw", (int)accel_report.z_raw); - warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2); + warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e93b1e331..8fd5679c9 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = lsm303d -SRCS = lsm303d.cpp +SRCS = lsm303d.cpp \ + iirFilter.c diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index d9268c0b3..122cf0cec 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -111,8 +111,9 @@ protected: ms5611::prom_s _prom; - struct work_s _work; - unsigned _measure_ticks; + struct hrt_call _baro_call; + + unsigned _call_baro_interval; unsigned _num_reports; volatile unsigned _next_report; @@ -143,12 +144,12 @@ protected: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop_cycle(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement @@ -166,12 +167,11 @@ protected: void cycle(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. + * Static trampoline; XXX is this needed? * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + void cycle_trampoline(void *arg); /** * Issue a measurement command for the current state. @@ -184,6 +184,7 @@ protected: * Collect the result of the most recent measurement. */ virtual int collect(); + }; /* @@ -195,7 +196,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : CDev("MS5611", BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), - _measure_ticks(0), + _call_baro_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), @@ -212,14 +213,13 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // work_cancel in stop_cycle called from the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + } MS5611::~MS5611() { /* make sure we are truly inactive */ - stop_cycle(); + stop(); /* free any existing reports */ if (_reports != nullptr) @@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) return -ENOSPC; /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_call_baro_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -298,41 +298,13 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ - do { - _measure_phase = 0; - _oldest_report = _next_report = 0; - - /* do temperature first */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* now do a pressure measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - usleep(MS5611_CONVERSION_INTERVAL); - - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + _measure_phase = 0; + _oldest_report = _next_report = 0; + measure(); - } while (0); + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); return ret; } @@ -347,8 +319,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: - stop_cycle(); - _measure_ticks = 0; + stop(); + _call_baro_interval = 0; return OK; /* external signalling not supported */ @@ -362,14 +334,15 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_call_baro_interval == 0); + + /* set interval to minimum legal value */ + _baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -377,21 +350,18 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) /* 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); + bool want_start = (_call_baro_interval == 0); /* check against maximum rate */ - if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + if (1000000/arg < MS5611_CONVERSION_INTERVAL) return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval */ + _baro_call.period = _call_baro_interval = 1000000/arg; /* if we need to start the poll state machine, do it */ if (want_start) - start_cycle(); + start(); return OK; } @@ -399,10 +369,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_call_baro_interval == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return (1000 / _call_baro_interval); case SENSORIOCSQUEUEDEPTH: { /* add one to account for the sentinel in the ring */ @@ -419,11 +389,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); + stop(); delete[] _reports; _num_reports = arg; _reports = buf; - start_cycle(); + start(); return OK; } @@ -457,30 +427,32 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start() { + /* make sure we are stopped first */ + stop(); - /* reset the report ring and state machine */ - _collect_phase = false; - _measure_phase = 0; + /* reset the report ring */ _oldest_report = _next_report = 0; + /* reset to first measure phase */ + _measure_phase = 0; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this); } void -MS5611::stop_cycle() +MS5611::stop() { - work_cancel(HPWORK, &_work); + hrt_cancel(&_baro_call); } void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = reinterpret_cast<MS5611 *>(arg); + MS5611 *dev = (MS5611 *)arg; - dev->cycle(); + cycle(); } void @@ -504,7 +476,7 @@ MS5611::cycle() //log("collection error %d", ret); } /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } @@ -517,14 +489,16 @@ MS5611::cycle() * doing pressure measurements at something close to the desired rate. */ if ((_measure_phase != 0) && - (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + (_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + // XXX maybe do something appropriate as well - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); +// /* schedule a fresh cycle call when we are ready to measure again */ +// work_queue(HPWORK, +// &_work, +// (worker_t)&MS5611::cycle_trampoline, +// this, +// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); return; } @@ -535,19 +509,12 @@ MS5611::cycle() if (ret != OK) { //log("measure error %d", ret); /* reset the collection state machine and try again */ - start_cycle(); + start(); return; } /* next phase is collection */ _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MS5611::cycle_trampoline, - this, - USEC2TICK(MS5611_CONVERSION_INTERVAL)); } int @@ -696,13 +663,14 @@ MS5611::collect() return OK; } + void MS5611::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", _call_baro_interval); printf("report queue: %u (%u/%u @ %p)\n", _num_reports, _oldest_report, _next_report, _reports); printf("TEMP: %d\n", _TEMP); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8f8555f1f..b019c4fc5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -256,7 +256,7 @@ private: * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. - * @return Zero if all values were successfully written. + * @return OK if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -266,7 +266,7 @@ private: * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. - * @return Zero if the value was written successfully. + * @return OK if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); @@ -277,7 +277,7 @@ private: * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. - * @return Zero if all values were successfully read. + * @return OK if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); @@ -1149,9 +1149,11 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - if (ret != num_values) + if (ret != num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } int @@ -1169,10 +1171,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } - int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); - if (ret != num_values) + int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); + if (ret != num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); - return ret; + return -1; + } + return OK; } uint32_t @@ -1180,7 +1184,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1)) + if (io_reg_get(page, offset, &value, 1) != OK) return _io_reg_get_error; return value; @@ -1193,7 +1197,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); - if (ret) + if (ret != OK) return ret; value &= ~clearbits; value |= setbits; @@ -1500,9 +1504,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - - *(uint32_t *)arg = value; + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; break; } @@ -1890,7 +1894,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[5]; /* work out what we're uploading... */ if (argc > 2) { @@ -1900,7 +1904,9 @@ px4io_main(int argc, char *argv[]) } else { fn[0] = "/fs/microsd/px4io.bin"; fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; + fn[2] = "/fs/microsd/px4io2.bin"; + fn[3] = "/etc/px4io2.bin"; + fn[4] = nullptr; } up = new PX4IO_Uploader; |