From d0f10a35505b0813ea5a63a67c8ad1cabeeeaffc Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 27 Aug 2012 19:09:15 -0700 Subject: driver work in progress --- apps/drivers/l3gd20/Makefile | 42 +++ apps/drivers/l3gd20/l3gd20.cpp | 822 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 864 insertions(+) create mode 100644 apps/drivers/l3gd20/Makefile create mode 100644 apps/drivers/l3gd20/l3gd20.cpp (limited to 'apps') diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile new file mode 100644 index 000000000..98e2d57c5 --- /dev/null +++ b/apps/drivers/l3gd20/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Makefile to build the L3GD20 driver. +# + +APPNAME = l3gd20 +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp new file mode 100644 index 000000000..747abd82e --- /dev/null +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -0,0 +1,822 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 Driver for the ST L3GD20 MEMS gyro connected via SPI. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } + +class L3GD20 : public device::SPI +{ +public: + L3GD20(int bus, spi_dev_e device); + ~L3GD20(); + + 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); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct gyro_report *_reports; + + struct gyro_scale _scale; + float _range_scale; + + unsigned _reads; + + unsigned _rate; + unsigned _range; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the L3GD20 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the L3GD20 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the L3GD20 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @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); + + /** + * Set the L3GD20 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); + + /** + * Set the L3GD20 lowpass filter. + * + * @param frequency Set the lowpass filter cutoff frequency to no less than + * this frequency. + * @return OK if the value can be supported. + */ + int set_bandwidth(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0xD4 +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_REFERENCE 0x25 +#define ADDR_OUT_TEMP 0x26 +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2A +#define ADDR_OUT_Y_H 0x2B +#define ADDR_OUT_Z_L 0x2C +#define ADDR_OUT_Z_H 0x2D +#define ADDR_FIFO_CTRL_REG 0x2E +#define ADDR_FIFO_SRC_REG 0x2F +#define ADDR_INT1_CFG 0x30 +#define ADDR_INT1_SRC 0x31 +#define ADDR_INT1_TSH_XH 0x32 +#define ADDR_INT1_TSH_XL 0x33 +#define ADDR_INT1_TSH_YH 0x34 +#define ADDR_INT1_TSH_YL 0x35 +#define ADDR_INT1_TSH_ZH 0x36 +#define ADDR_INT1_TSH_ZL 0x37 +#define ADDR_INT1_DURATION 0x38 + +#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ +#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ + +/* Internal configuration values */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define REG4_BDU (1<<7) +#define REG4_BLE (1<<6) +//#define REG4_SPI_3WIRE (1<<0) + +#define REG5_FIFO_ENABLE (1<<6) +#define REG5_REBOOT_MEMORY (1<<7) + +#define STATUS_ZYXOR (1<<7) +#define STATUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define FIFO_CTRL_BYPASS_MODE (0<<5) +#define FIFO_CTRL_FIFO_MODE (1<<5) +#define FIFO_CTRL_STREAM_MODE (1<<6) +#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) +#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) + +#define L3GD20_RANGE_250DPS (0<<4) +#define L3GD20_RANGE_500DPS (1<<4) +#define L3GD20_RANGE_2000DPS (3<<4) + +#define L3GD20_RATE_95HZ ((0<<6) | (0<<4)) +#define L3GD20_RATE_190HZ ((1<<6) | (0<<4)) +#define L3GD20_RATE_380HZ ((2<<6) | (1<<4)) +#define L3GD20_RATE_760HZ ((3<<6) | (2<<4)) + +/* + * Driver 'main' command. + */ +extern "C" { int l3gd20_main(int argc, char *argv[]); } + + +L3GD20::L3GD20(int bus, spi_dev_e device) : + SPI("L3GD20", GYRO_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _reads(0), + _rate(L3GD20_RATE_760HZ), + _range(L3GD20_RANGE_2000DPS) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors XXX + _scale.x_offset = 0; + _scale.x_scale = 1.0f; + _scale.y_offset = 0; + _scale.y_scale = 1.0f; + _scale.z_offset = 0; + _scale.z_scale = 1.0f; +} + +L3GD20::~L3GD20() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +L3GD20::init() +{ + int ret = ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _oldest_report = _next_report = 0; + _reports = new struct gyro_report[_num_reports]; + if (_reports == nullptr) + goto out; + + /* 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, 0x10); + write_reg(ADDR_CTRL_REG5, 0); + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + if ((set_range(L3GD20_RANGE_500DPS) != 0) || + (set_rate(L3GD20_RATE_760HZ_LP_100HZ) != 0)) + goto out; + + ret = OK; +out: + return ret; +} + +int +L3GD20::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + return OK; + + return -EIO; +} + +ssize_t +L3GD20::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct gyro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + _reads++; + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + return ret; +} + +int +L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* XXX 500Hz is just a wild guess */ + return ioctl(filp, SENSORIOCSPOLLRATE, 500); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) + return SENSOR_POLLRATE_MANUAL; + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct gyro_report *buf = new struct gyro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports -1; + + case SENSORIOCRESET: + /* XXX implement */ + return -EINVAL; + + case GYROIOCSSAMPLERATE: + case GYROIOCGSAMPLERATE: + /* XXX not implemented */ + return -EINVAL; + + case GYROIOCSLOWPASS: + case GYROIOCGLOWPASS: + /* XXX not implemented */ + return -EINVAL; + + case GYROIOCSSCALE: + case GYROIOCGSCALE: + /* XXX not implemented */ + return -EINVAL; + + case GYROIOCSRANGE: + case GYROIOCGRANGE: + /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_m_s2 = xx + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); +} + +uint8_t +L3GD20::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +L3GD20::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +L3GD20::set_range(unsigned max_g) +{ +#if 0 + uint8_t rangebits; + float rangescale; + + if (max_g > 16) { + return -ERANGE; + + } else if (max_g > 8) { /* 16G */ + rangebits = OFFSET_LSB1_RANGE_16G; + rangescale = 1.98; + + } else if (max_g > 4) { /* 8G */ + rangebits = OFFSET_LSB1_RANGE_8G; + rangescale = 0.99; + + } else if (max_g > 3) { /* 4G */ + rangebits = OFFSET_LSB1_RANGE_4G; + rangescale = 0.5; + + } else if (max_g > 2) { /* 3G */ + rangebits = OFFSET_LSB1_RANGE_3G; + rangescale = 0.38; + + } else if (max_g > 1) { /* 2G */ + rangebits = OFFSET_LSB1_RANGE_2G; + rangescale = 0.25; + + } else { /* 1G */ + rangebits = OFFSET_LSB1_RANGE_1G; + rangescale = 0.13; + } + + /* adjust sensor configuration */ + modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); + _range_scale = rangescale; +#endif + return OK; +} + +int +L3GD20::set_bandwidth(unsigned frequency) +{ +#if 0 + uint8_t bwbits; + + if (frequency > 1200) { + return -ERANGE; + + } else if (frequency > 600) { + bwbits = BW_TCS_BW_1200HZ; + + } else if (frequency > 300) { + bwbits = BW_TCS_BW_600HZ; + + } else if (frequency > 150) { + bwbits = BW_TCS_BW_300HZ; + + } else if (frequency > 75) { + bwbits = BW_TCS_BW_150HZ; + + } else if (frequency > 40) { + bwbits = BW_TCS_BW_75HZ; + + } else if (frequency > 20) { + bwbits = BW_TCS_BW_40HZ; + + } else if (frequency > 10) { + bwbits = BW_TCS_BW_20HZ; + + } else { + bwbits = BW_TCS_BW_10HZ; + } + + /* adjust sensor configuration */ + modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits); +#endif + return OK; +} + +void +L3GD20::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); +} + +void +L3GD20::stop() +{ + hrt_cancel(&_call); +} + +void +L3GD20::measure_trampoline(void *arg) +{ + L3GD20 *dev = (L3GD20 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +L3GD20::measure() +{ + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_report; +#pragma pack(pop) + + gyro_report *report = &_reports[_next_report]; + + /* start the performance counter */ + perf_begin(_sample_perf); + + /* fetch data from the sensor */ + report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; + transfer(&report, &report, sizeof(report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + report->timestamp = hrt_absolute_time(); + /* XXX adjust for sensor alignment to board here */ + report->raw_x = raw_report.x; + report->raw_y = raw_report.y; + report->raw_z = raw_report.z; + + report->x = ((raw_report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; + report->y = ((raw_report.y * _range_scale) - _scale.y_offset) * _scale.y_scale; + report->z = ((raw_report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + report->scaling = _range_scale; + report->range_rad_s = _range_rad_s; + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + + /* stop the perf counter */ + perf_end(_sample_perf); +} + +void +L3GD20::print_info() +{ + printf("reads: %u\n", _reads); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +L3GD20 *g_dev; + +/* + * XXX this should just be part of the generic sensors test... + */ + +int +test() +{ + int fd = -1; + struct gyro_report report; + ssize_t sz; + const char *reason = "test OK"; + + do { + + /* get the driver */ + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + reason = "can't open driver"; + break; + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + reason = "immediate read failed"; + break; + } + + printf("single read\n"); + fflush(stdout); + printf("time: %lld\n", report.timestamp); + printf("x: %f\n", report.x); + printf("y: %f\n", report.y); + printf("z: %f\n", report.z); + + } while (0); + + printf("L3GD20: %s\n", reason); + + return OK; +} + +int +info() +{ + if (g_dev == nullptr) { + fprintf(stderr, "L3GD20: driver not running\n"); + return -ENOENT; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return OK; +} + + +} // namespace + +int +l3gd20_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "L3GD20: already loaded\n"); + return -EBUSY; + } + + /* create the driver */ + g_dev = new L3GD20(CONFIG_L3GD20_SPI_BUS, (spi_dev_e)CONFIG_L3GD20_SPI_DEVICE); + + if (g_dev == nullptr) { + fprintf(stderr, "L3GD20: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "L3GD20: driver init failed\n"); + usleep(100000); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + printf("L3GD20: driver started\n"); + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + return test(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); + return -EINVAL; +} -- cgit v1.2.3