aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/l3gd20/l3gd20.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp122
1 files changed, 94 insertions, 28 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 90c3db9ae..64d1a7e55 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -34,6 +34,9 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ *
+ * Note: With the exception of the self-test feature, the ST L3G4200D is
+ * also supported by this driver.
*/
#include <nuttx/config.h>
@@ -51,6 +54,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
+#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@@ -65,6 +69,7 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
@@ -89,9 +94,11 @@ static const int ERROR = -1;
#define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM_H 0xD7
#define WHO_I_AM 0xD4
+#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
#define ADDR_CTRL_REG1 0x20
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
+
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
@@ -166,15 +173,20 @@ static const int ERROR = -1;
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
#define L3GD20_DEFAULT_RATE 760
+#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
+#ifndef SENSOR_BOARD_ROTATION_DEFAULT
+#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
+#endif
+
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
{
public:
- L3GD20(int bus, const char* path, spi_dev_e device);
+ L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
virtual ~L3GD20();
virtual int init();
@@ -216,6 +228,11 @@ private:
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
+ /* true if an L3G4200D is detected */
+ bool _is_l3g4200d;
+
+ enum Rotation _rotation;
+
/**
* Start automatic measurement.
*/
@@ -315,7 +332,7 @@ private:
int self_test();
};
-L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
+L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call_interval(0),
_reports(nullptr),
@@ -324,14 +341,16 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_topic(-1),
_class_instance(-1),
_current_rate(0),
- _orientation(SENSOR_BOARD_ROTATION_270_DEG),
+ _orientation(SENSOR_BOARD_ROTATION_DEFAULT),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
_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)
+ _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+ _is_l3g4200d(false),
+ _rotation(rotation)
{
// enable debug() calls
_debug_enabled = true;
@@ -413,14 +432,7 @@ L3GD20::probe()
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
- #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
- _orientation = SENSOR_BOARD_ROTATION_270_DEG;
- #elif CONFIG_ARCH_BOARD_PX4FMU_V2
- _orientation = SENSOR_BOARD_ROTATION_270_DEG;
- #else
- #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
- #endif
-
+ _orientation = SENSOR_BOARD_ROTATION_DEFAULT;
success = true;
}
@@ -430,6 +442,13 @@ L3GD20::probe()
success = true;
}
+ /* Detect the L3G4200D used on AeroCore */
+ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+ _is_l3g4200d = true;
+ _orientation = SENSOR_BOARD_ROTATION_DEFAULT;
+ success = true;
+ }
+
if (success)
return OK;
@@ -502,6 +521,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
+ if (_is_l3g4200d) {
+ return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
+ }
return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
@@ -683,23 +705,26 @@ L3GD20::set_samplerate(unsigned frequency)
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
if (frequency == 0)
- frequency = 760;
+ frequency = _is_l3g4200d ? 800 : 760;
- /* use limits good for H or non-H models */
+ /*
+ * Use limits good for H or non-H models. Rates are slightly different
+ * for L3G4200D part but register settings are the same.
+ */
if (frequency <= 100) {
- _current_rate = 95;
+ _current_rate = _is_l3g4200d ? 100 : 95;
bits |= RATE_95HZ_LP_25HZ;
} else if (frequency <= 200) {
- _current_rate = 190;
+ _current_rate = _is_l3g4200d ? 200 : 190;
bits |= RATE_190HZ_LP_50HZ;
} else if (frequency <= 400) {
- _current_rate = 380;
+ _current_rate = _is_l3g4200d ? 400 : 380;
bits |= RATE_380HZ_LP_50HZ;
} else if (frequency <= 800) {
- _current_rate = 760;
+ _current_rate = _is_l3g4200d ? 800 : 760;
bits |= RATE_760HZ_LP_50HZ;
} else {
return -EINVAL;
@@ -772,7 +797,7 @@ L3GD20::reset()
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
- set_samplerate(0); // 760Hz
+ set_samplerate(0); // 760Hz or 800Hz
set_range(L3GD20_DEFAULT_RANGE_DPS);
set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
@@ -801,7 +826,7 @@ L3GD20::measure()
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
- if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+ if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
@@ -894,6 +919,9 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
+ // apply user specified rotation
+ rotate_3f(_rotation, report.x, report.y, report.z);
+
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
@@ -954,7 +982,8 @@ namespace l3gd20
L3GD20 *g_dev;
-void start();
+void usage();
+void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
@@ -963,7 +992,7 @@ void info();
* Start the driver.
*/
void
-start()
+start(bool external_bus, enum Rotation rotation)
{
int fd;
@@ -971,7 +1000,15 @@ start()
errx(0, "already started");
/* create the driver */
- g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+ if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+ g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation);
+#else
+ errx(0, "External SPI not available");
+#endif
+ } else {
+ g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation);
+ }
if (g_dev == nullptr)
goto fail;
@@ -1083,35 +1120,64 @@ info()
exit(0);
}
+void
+usage()
+{
+ warnx("missing command: try 'start', 'info', 'test', 'reset'");
+ warnx("options:");
+ warnx(" -X (external bus)");
+ warnx(" -R rotation");
+}
} // namespace
int
l3gd20_main(int argc, char *argv[])
{
+ bool external_bus = false;
+ int ch;
+ enum Rotation rotation = ROTATION_NONE;
+
+ /* jump over start/off/etc and look at options first */
+ while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+ switch (ch) {
+ case 'X':
+ external_bus = true;
+ break;
+ case 'R':
+ rotation = (enum Rotation)atoi(optarg);
+ break;
+ default:
+ l3gd20::usage();
+ exit(0);
+ }
+ }
+
+ const char *verb = argv[optind];
+
/*
* Start/load the driver.
*/
- if (!strcmp(argv[1], "start"))
- l3gd20::start();
+ if (!strcmp(verb, "start"))
+ l3gd20::start(external_bus, rotation);
/*
* Test the driver/device.
*/
- if (!strcmp(argv[1], "test"))
+ if (!strcmp(verb, "test"))
l3gd20::test();
/*
* Reset the driver.
*/
- if (!strcmp(argv[1], "reset"))
+ if (!strcmp(verb, "reset"))
l3gd20::reset();
/*
* Print driver information.
*/
- if (!strcmp(argv[1], "info"))
+ if (!strcmp(verb, "info"))
l3gd20::info();
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-19 04:14:34 +0000