aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/boards/px4fmuv2/module.mk5
-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.c85
-rw-r--r--src/drivers/led/led.cpp4
-rw-r--r--src/drivers/lsm303d/iirFilter.c255
-rw-r--r--src/drivers/lsm303d/iirFilter.h59
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp120
-rw-r--r--src/drivers/lsm303d/module.mk3
-rw-r--r--src/drivers/ms5611/ms5611.cpp148
-rw-r--r--src/drivers/px4io/px4io.cpp36
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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-07 15:23:05 +0000