Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 22:24:47 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-21 11:24:51 +02:00
parent f868c99f06 73521cbc66
commit 28171fb596
19 changed files with 353 additions and 2973 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/device/cdev.cpp
+2 -2
View File
@@ -74,7 +74,7 @@ static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
* Note that we use the GNU extension syntax here because we don't get designated
* initialisers in gcc 4.6.
*/
static const struct file_operations cdev_fops = {
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
@@ -118,7 +118,7 @@ CDev::init()
goto out;
// now register the driver
ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
goto out;
apps/drivers/device/device.h
+6
View File
@@ -286,6 +286,12 @@ public:
bool is_open() { return _open_count > 0; }
protected:
/**
* Pointer to the default cdev file operations table; useful for
* registering clone devices etc.
*/
static const struct file_operations fops;
/**
* Check the current state of the device for poll events from the
* perspective of the file.
apps/px4/fmu/fmu.cpp
+34 -17
View File
@@ -79,7 +79,7 @@ public:
FMUServo(Mode mode, int update_rate);
~FMUServo();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init();
@@ -93,6 +93,7 @@ private:
int _t_armed;
orb_advert_t _t_outputs;
unsigned _num_outputs;
bool _primary_pwm_device;
volatile bool _task_should_exit;
bool _armed;
@@ -118,7 +119,7 @@ FMUServo *g_servo;
} // namespace
FMUServo::FMUServo(Mode mode, int update_rate) :
CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
CDev("fmuservo", "/dev/px4fmu"),
_mode(mode),
_update_rate(update_rate),
_task(-1),
@@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
_t_armed(-1),
_t_outputs(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
FMUServo::~FMUServo()
{
if (_task != -1) {
/* task should wake up every 100ms or so at least */
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 0;
unsigned i = 10;
do {
/* wait 20ms */
usleep(20000);
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (++i > 10) {
if (--i == 0) {
task_delete(_task);
break;
}
@@ -154,6 +154,10 @@ FMUServo::~FMUServo()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
g_servo = nullptr;
}
@@ -170,6 +174,13 @@ FMUServo::init()
if (ret != OK)
return ret;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_spawn("fmuservo",
SCHED_DEFAULT,
@@ -216,8 +227,12 @@ FMUServo::task_main()
break;
}
/* subscribe to objects that we are interested in watching */
_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, update_rate_in_ms);
@@ -226,11 +241,13 @@ FMUServo::task_main()
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
struct actuator_outputs_s outputs;
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
struct pollfd fds[2];
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
@@ -282,7 +299,7 @@ FMUServo::task_main()
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
struct actuator_armed_s aa;
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
}
int
FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
@@ -569,7 +586,7 @@ fake(int argc, char *argv[])
exit(1);
}
struct actuator_controls_s ac;
actuator_controls_s ac;
ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
apps/px4/px4io/driver/px4io.cpp
+306 -259
View File
File diff suppressed because it is too large Load Diff
apps/px4/px4io/driver/uploader.cpp
+1 -1
View File
@@ -318,7 +318,7 @@ PX4IO_Uploader::verify()
ret = recv(c);
if (ret != OK) {
log("%d: got %d waiting for bytes", ret);
log("%d: got %d waiting for bytes", base + i, ret);
return ret;
}
apps/px4/tests/test_sensors.c
-330
View File
@@ -56,10 +56,6 @@
#include "tests.h"
#include <arch/board/drv_lis331.h>
#include <arch/board/drv_bma180.h>
#include <arch/board/drv_l3gd20.h>
#include <arch/board/drv_hmc5883l.h>
#include <drivers/drv_accel.h>
/****************************************************************************
@@ -75,10 +71,6 @@
****************************************************************************/
//static int lis331(int argc, char *argv[]);
static int l3gd20(int argc, char *argv[]);
static int bma180(int argc, char *argv[]);
static int hmc5883l(int argc, char *argv[]);
static int ms5611(int argc, char *argv[]);
static int mpu6000(int argc, char *argv[]);
/****************************************************************************
@@ -90,12 +82,7 @@ struct {
const char *path;
int (* test)(int argc, char *argv[]);
} sensors[] = {
{"bma180", "/dev/bma180", bma180},
{"mpu6000", "/dev/accel", mpu6000},
{"l3gd20", "/dev/l3gd20", l3gd20},
{"hmc5883l", "/dev/hmc5883l", hmc5883l},
{"ms5611", "/dev/ms5611", ms5611},
// {"lis331", "/dev/lis331", lis331},
{NULL, NULL, NULL}
};
@@ -107,226 +94,6 @@ struct {
* Private Functions
****************************************************************************/
//static int
//lis331(int argc, char *argv[])
//{
// int fd;
// int16_t buf[3];
// int ret;
//
// fd = open("/dev/lis331", O_RDONLY);
// if (fd < 0) {
// printf("\tlis331: not present on PX4FMU v1.5 and later\n");
// return ERROR;
// }
//
// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
//
// printf("LIS331: ioctl fail\n");
// return ERROR;
// }
//
// /* wait at least 100ms, sensor should have data after no more than 20ms */
// usleep(100000);
//
// /* read data - expect samples */
// ret = read(fd, buf, sizeof(buf));
// if (ret != sizeof(buf)) {
// printf("LIS331: read1 fail (%d)\n", ret);
// return ERROR;
// }
//
// /* read data - expect no samples (should not be ready again yet) */
// ret = read(fd, buf, sizeof(buf));
// if (ret != 0) {
// printf("LIS331: read2 fail (%d)\n", ret);
// return ERROR;
// }
//
// /* XXX more tests here */
//
// return 0;
//}
static int
l3gd20(int argc, char *argv[])
{
printf("\tL3GD20: test start\n");
fflush(stdout);
int fd;
int16_t buf[3] = {0, 0, 0};
int ret;
fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK);
if (fd < 0) {
printf("L3GD20: open fail\n");
return ERROR;
}
// if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) ||
// ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) {
//
// printf("L3GD20: ioctl fail\n");
// return ERROR;
// } else {
// printf("\tconfigured..\n");
// }
//
// /* wait at least 100ms, sensor should have data after no more than 2ms */
// usleep(100000);
/* read data - expect samples */
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf));
//return ERROR;
} else {
printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
}
/* wait at least 2 ms, sensor should have data after no more than 1.5ms */
usleep(2000);
/* read data - expect no samples (should not be ready again yet) */
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tL3GD20: read2 fail (%d)\n", ret);
close(fd);
return ERROR;
} else {
printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
}
/* empty sensor buffer */
ret = 0;
while (ret != sizeof(buf)) {
// Keep reading until successful
ret = read(fd, buf, sizeof(buf));
}
/* test if FIFO is operational */
usleep(14800); // Expecting 10 measurements
ret = 0;
int count = 0;
bool dataready = true;
while (dataready) {
// Keep reading until successful
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
dataready = false;
} else {
count++;
}
}
printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count);
/* read data - expect no samples (should not be ready again yet) */
ret = read(fd, buf, sizeof(buf));
if (ret != 0) {
printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret);
// return ERROR;
}
close(fd);
/* Let user know everything is ok */
printf("\tOK: L3GD20 passed all tests successfully\n");
return OK;
}
static int
bma180(int argc, char *argv[])
{
// XXX THIS SENSOR IS OBSOLETE
// TEST REMAINS, BUT ALWAYS RETURNS OK
printf("\tBMA180: test start\n");
fflush(stdout);
int fd;
int16_t buf[3] = {0, 0, 0};
int ret;
fd = open("/dev/bma180", O_RDONLY);
if (fd < 0) {
printf("\tBMA180: open fail\n");
return OK;
}
// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) ||
// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) {
//
// printf("BMA180: ioctl fail\n");
// return ERROR;
// }
//
/* wait at least 100ms, sensor should have data after no more than 20ms */
usleep(100000);
/* read data - expect samples */
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tBMA180: read1 fail (%d)\n", ret);
close(fd);
return OK;
} else {
printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
}
/* wait at least 10ms, sensor should have data after no more than 2ms */
usleep(100000);
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tBMA180: read2 fail (%d)\n", ret);
close(fd);
return OK;
} else {
printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
}
/* empty sensor buffer */
ret = 0;
while (ret != sizeof(buf)) {
// Keep reading until successful
ret = read(fd, buf, sizeof(buf));
}
ret = read(fd, buf, sizeof(buf));
if (ret != 0) {
printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret);
}
/* Let user know everything is ok */
printf("\tOK: BMA180 passed all tests successfully\n");
close(fd);
return OK;
}
static int
mpu6000(int argc, char *argv[])
{
@@ -379,103 +146,6 @@ mpu6000(int argc, char *argv[])
return OK;
}
static int
ms5611(int argc, char *argv[])
{
printf("\tMS5611: test start\n");
fflush(stdout);
int fd;
float buf[3] = {0.0f, 0.0f, 0.0f};
int ret;
fd = open("/dev/ms5611", O_RDONLY);
if (fd < 0) {
printf("\tMS5611: open fail\n");
return ERROR;
}
for (int i = 0; i < 5; i++) {
/* read data - expect samples */
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
if ((int8_t)ret == -EAGAIN || (int8_t)ret == -EINPROGRESS) {
/* waiting for device to become ready, this is not an error */
} else {
printf("\tMS5611: read fail (%d)\n", ret);
close(fd);
return ERROR;
}
} else {
/* hack for float printing */
int32_t pressure_int = buf[0];
int32_t altitude_int = buf[1];
int32_t temperature_int = buf[2];
printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100));
}
/* wait at least 10ms, sensor should have data after no more than 6.5ms */
usleep(10000);
}
close(fd);
/* Let user know everything is ok */
printf("\tOK: MS5611 passed all tests successfully\n");
return OK;
}
static int
hmc5883l(int argc, char *argv[])
{
printf("\tHMC5883L: test start\n");
fflush(stdout);
int fd;
int16_t buf[7] = {0, 0, 0};
int ret;
fd = open("/dev/hmc5883l", O_RDONLY);
if (fd < 0) {
printf("\tHMC5883L: open fail\n");
return ERROR;
}
int i;
for (i = 0; i < 5; i++) {
/* wait at least 7ms, sensor should have data after no more than 6.5ms */
usleep(7000);
/* read data - expect samples */
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]);
close(fd);
return ERROR;
} else {
printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]);
}
}
close(fd);
/* Let user know everything is ok */
printf("\tOK: HMC5883L passed all tests successfully\n");
return OK;
}
/****************************************************************************
* Public Functions
****************************************************************************/
apps/px4io/protocol.h
+2
View File
@@ -39,6 +39,8 @@
* messages and the corresponding complexity involved.
*/
#pragma once
/*
* XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
* TREES ARE MERGED.
nuttx/configs/px4fmu/include/drv_bma180.h
-99
View File
@@ -1,99 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the BOSCH BMA180 MEMS accelerometer
*/
/* IMPORTANT NOTES:
*
* SPI max. clock frequency: 25 Mhz
* CS has to be high before transfer,
* go low right before transfer and
* go high again right after transfer
*
*/
#include <sys/ioctl.h>
#define _BMA180BASE 0x6300
#define BMA180C(_x) _IOC(_BMA180BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define BMA180_SETRATE BMA180C(1)
#define BMA180_RATE_LP_10HZ (0<<4)
#define BMA180_RATE_LP_20HZ (1<<4)
#define BMA180_RATE_LP_40HZ (2<<4)
#define BMA180_RATE_LP_75HZ (3<<4)
#define BMA180_RATE_LP_150HZ (4<<4)
#define BMA180_RATE_LP_300HZ (5<<4)
#define BMA180_RATE_LP_600HZ (6<<4)
#define BMA180_RATE_LP_1200HZ (7<<4)
/*
* Sets the sensor internal range.
*/
#define BMA180_SETRANGE BMA180C(2)
#define BMA180_RANGE_1G (0<<1)
#define BMA180_RANGE_1_5G (1<<1)
#define BMA180_RANGE_2G (2<<1)
#define BMA180_RANGE_3G (3<<1)
#define BMA180_RANGE_4G (4<<1)
#define BMA180_RANGE_8G (5<<1)
#define BMA180_RANGE_16G (6<<1)
/*
* Sets the address of a shared BMA180_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define BMA180_SETBUFFER BMA180C(3)
struct bma180_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint16_t x;
uint16_t y;
uint16_t z;
uint8_t temp;
} samples[];
};
extern int bma180_attach(struct spi_dev_s *spi, int spi_id);
nuttx/configs/px4fmu/include/drv_hmc5883l.h
-100
View File
@@ -1,100 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST HMC5883L gyroscope
*/
#include <sys/ioctl.h>
#define _HMC5883LBASE 0x6100
#define HMC5883LC(_x) _IOC(_HMC5883LBASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define HMC5883L_SETRATE HMC5883LC(1)
/* set rate (configuration A register */
#define HMC5883L_RATE_0_75HZ (0 << 2) /* 0.75 Hz */
#define HMC5883L_RATE_1_50HZ (1 << 2) /* 1.5 Hz */
#define HMC5883L_RATE_3_00HZ (2 << 2) /* 3 Hz */
#define HMC5883L_RATE_7_50HZ (3 << 2) /* 7.5 Hz */
#define HMC5883L_RATE_15HZ (4 << 2) /* 15 Hz (default) */
#define HMC5883L_RATE_30HZ (5 << 2) /* 30 Hz */
#define HMC5883L_RATE_75HZ (6 << 2) /* 75 Hz */
/*
* Sets the sensor internal range.
*/
#define HMC5883L_SETRANGE HMC5883LC(2)
#define HMC5883L_RANGE_0_88GA (0 << 5)
#define HMC5883L_RANGE_1_33GA (1 << 5)
#define HMC5883L_RANGE_1_90GA (2 << 5)
#define HMC5883L_RANGE_2_50GA (3 << 5)
#define HMC5883L_RANGE_4_00GA (4 << 5)
/*
* Set the sensor measurement mode.
*/
#define HMC5883L_MODE_NORMAL (0 << 0)
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0)
#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1)
/*
* Sets the address of a shared HMC5883L_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define HMC5883L_SETBUFFER HMC5883LC(3)
struct hmc5883l_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
int16_t x;
int16_t y;
int16_t z;
} samples[];
};
#define HMC5883L_RESET HMC5883LC(4)
#define HMC5883L_CALIBRATION_ON HMC5883LC(5)
#define HMC5883L_CALIBRATION_OFF HMC5883LC(6)
extern int hmc5883l_attach(struct i2c_dev_s *i2c);
nuttx/configs/px4fmu/include/drv_l3gd20.h
-108
View File
@@ -1,108 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST L3GD20 gyroscope
*/
/* IMPORTANT NOTES:
*
* SPI max. clock frequency: 10 Mhz
* CS has to be high before transfer,
* go low right before transfer and
* go high again right after transfer
*
*/
#include <sys/ioctl.h>
#define _L3GD20BASE 0x6200
#define L3GD20C(_x) _IOC(_L3GD20BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define L3GD20_SETRATE L3GD20C(1)
#define L3GD20_RATE_95HZ_LP_12_5HZ ((0<<7) | (0<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_190HZ_LP_12_5HZ ((0<<7) | (1<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define L3GD20_RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define L3GD20_RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
#define L3GD20_RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
#define L3GD20_RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
#define L3GD20_RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
/*
* Sets the sensor internal range.
*/
#define L3GD20_SETRANGE L3GD20C(2)
#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))
/*
* Sets the address of a shared l3gd20_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define L3GD20_SETBUFFER L3GD20C(3)
struct l3gd20_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
int16_t x;
int16_t y;
int16_t z;
} samples[];
};
extern int l3gd20_attach(struct spi_dev_s *spi, int spi_id);
nuttx/configs/px4fmu/include/drv_lis331.h
-83
View File
@@ -1,83 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST LIS331 MEMS accelerometer
*/
#include <sys/ioctl.h>
#define _LIS331BASE 0x6900
#define LIS331C(_x) _IOC(_LIS331BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define LIS331_SETRATE LIS331C(1)
#define LIS331_RATE_50Hz (0<<3)
#define LIS331_RATE_100Hz (1<<3)
#define LIS331_RATE_400Hz (2<<3)
#define LIS331_RATE_1000Hz (3<<3)
/*
* Sets the sensor internal range.
*/
#define LIS331_SETRANGE LIS331C(2)
#define LIS331_RANGE_2G (0<<4)
#define LIS331_RANGE_4G (1<<4)
#define LIS331_RANGE_8G (3<<4)
/*
* Sets the address of a shared lis331_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define LIS331_SETBUFFER LIS331C(3)
struct lis331_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint16_t x;
uint16_t y;
uint16_t z;
} samples[];
};
extern int lis331_attach(struct spi_dev_s *spi, int spi_id);
nuttx/configs/px4fmu/include/drv_ms5611.h
-76
View File
@@ -1,76 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the Meas Spec MS5611 barometric pressure sensor
*/
#include <sys/ioctl.h>
#define _MS5611BASE 0x6A00
#define MS5611C(_x) _IOC(_MS5611BASE, _x)
/*
* Sets the sensor internal sampling rate, and if a buffer
* has been configured, the rate at which entries will be
* added to the buffer.
*/
#define MS5611_SETRATE MS5611C(1)
/* set rate (configuration A register */
#define MS5611_RATE_0_75HZ (0 << 2) /* 0.75 Hz */
/*
* Sets the sensor internal range.
*/
#define MS5611_SETRANGE MS5611C(2)
#define MS5611_RANGE_0_88GA (0 << 5)
/*
* Sets the address of a shared MS5611_buffer
* structure that is maintained by the driver.
*
* If zero is passed as the address, disables
* the buffer updating.
*/
#define MS5611_SETBUFFER MS5611C(3)
struct ms5611_buffer {
uint32_t size; /* number of entries in the samples[] array */
uint32_t next; /* the next entry that will be populated */
struct {
uint32_t pressure;
uint16_t temperature;
} samples[];
};
extern int ms5611_attach(struct i2c_dev_s *i2c);
nuttx/configs/px4fmu/src/Makefile
+1 -1
View File
@@ -41,7 +41,7 @@ ASRCS =
AOBJS = $(ASRCS:.S=$(OBJEXT))
CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \
drv_gpio.c drv_bma180.c drv_l3gd20.c \
drv_gpio.c \
drv_led.c drv_eeprom.c \
drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
up_cpuload.c
nuttx/configs/px4fmu/src/drv_bma180.c
-341
View File
@@ -1,341 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
*/
/*
* Driver for the Bosch BMA 180 MEMS accelerometer
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/spi.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include <stdio.h>
#include "chip.h"
#include "px4fmu-internal.h"
#include <arch/board/drv_bma180.h>
/*
* BMA180 registers
*/
/* Important Notes:
*
* - MAX SPI clock: 25 MHz
* - Readout time: 0.417 ms in high accuracy mode
* - Boot / ready time: 1.27 ms
*
*/
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
#define ADDR_VERSION 0x01
#define ADDR_CTRL_REG0 0x0D
#define ADDR_CTRL_REG1 0x0E
#define ADDR_CTRL_REG2 0x0F
#define ADDR_BWTCS 0x20
#define ADDR_CTRL_REG3 0x21
#define ADDR_CTRL_REG4 0x22
#define ADDR_OLSB1 0x35
#define ADDR_ACC_X_LSB 0x02
#define ADDR_ACC_Z_MSB 0x07
#define ADDR_TEMPERATURE 0x08
#define ADDR_STATUS_REG1 0x09
#define ADDR_STATUS_REG2 0x0A
#define ADDR_STATUS_REG3 0x0B
#define ADDR_STATUS_REG4 0x0C
#define ADDR_RESET 0x10
#define SOFT_RESET 0xB6
#define ADDR_DIS_I2C 0x27
#define REG0_WRITE_ENABLE 0x10
#define RANGEMASK 0x0E
#define BWMASK 0xF0
static ssize_t bma180_read(struct file *filp, FAR char *buffer, size_t buflen);
static int bma180_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations bma180_fops = {
.read = bma180_read,
.ioctl = bma180_ioctl,
};
struct bma180_dev_s
{
struct spi_dev_s *spi;
int spi_id;
uint8_t rate;
struct bma180_buffer *buffer;
};
static struct bma180_dev_s bma180_dev;
static void bma180_write_reg(uint8_t address, uint8_t data);
static uint8_t bma180_read_reg(uint8_t address);
static bool read_fifo(uint16_t *data);
static int bma180_set_range(uint8_t range);
static int bma180_set_rate(uint8_t rate);
static void
bma180_write_reg(uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, true);
SPI_SNDBLOCK(bma180_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, false);
}
static uint8_t
bma180_read_reg(uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, true);
SPI_EXCHANGE(bma180_dev.spi, cmd, data, sizeof(cmd));
SPI_SELECT(bma180_dev.spi, bma180_dev.spi_id, false);
return data[1];
}
static bool
read_fifo(uint16_t *data)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
int16_t x;
int16_t y;
int16_t z;
uint8_t temp;
} __attribute__((packed)) report;
report.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT;
SPI_LOCK(bma180_dev.spi, true);
report.x = bma180_read_reg(ADDR_ACC_X_LSB);
report.x |= (bma180_read_reg(ADDR_ACC_X_LSB+1) << 8);
report.y = bma180_read_reg(ADDR_ACC_X_LSB+2);
report.y |= (bma180_read_reg(ADDR_ACC_X_LSB+3) << 8);
report.z = bma180_read_reg(ADDR_ACC_X_LSB+4);
report.z |= (bma180_read_reg(ADDR_ACC_X_LSB+5) << 8);
report.temp = bma180_read_reg(ADDR_ACC_X_LSB+6);
SPI_LOCK(bma180_dev.spi, false);
/* Collect status and remove two top bits */
uint8_t new_data = (report.x & 0x01) + (report.x & 0x01) + (report.x & 0x01);
report.x = (report.x >> 2);
report.y = (report.y >> 2);
report.z = (report.z >> 2);
data[0] = report.x;
data[1] = report.y;
data[2] = report.z;
/* return 1 for all three axes new */
return (new_data > 0); // bit funky, depends on timing
}
static int
bma180_set_range(uint8_t range)
{
/* enable writing to chip config */
uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 |= REG0_WRITE_ENABLE;
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
/* set range */
uint8_t olsb1 = bma180_read_reg(ADDR_OLSB1);
olsb1 &= (~RANGEMASK);
olsb1 |= (range);// & RANGEMASK);
bma180_write_reg(ADDR_OLSB1, olsb1);
// up_udelay(500);
/* block writing to chip config */
ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 &= (~REG0_WRITE_ENABLE);
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
uint8_t new_olsb1 = bma180_read_reg(ADDR_OLSB1);
/* return 0 on success, 1 on failure */
return !(olsb1 == new_olsb1);
}
static int
bma180_set_rate(uint8_t rate)
{
/* enable writing to chip config */
uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 |= REG0_WRITE_ENABLE;
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
/* set rate / bandwidth */
uint8_t bwtcs = bma180_read_reg(ADDR_BWTCS);
bwtcs &= (~BWMASK);
bwtcs |= (rate);// & BWMASK);
bma180_write_reg(ADDR_BWTCS, bwtcs);
// up_udelay(500);
/* block writing to chip config */
ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 &= (~REG0_WRITE_ENABLE);
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
uint8_t new_bwtcs = bma180_read_reg(ADDR_BWTCS);
/* return 0 on success, 1 on failure */
return !(bwtcs == new_bwtcs);
}
static ssize_t
bma180_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 6) {
if (read_fifo((uint16_t *)buffer))
return 6;
/* no data */
return 0;
}
/* buffer too small */
errno = ENOSPC;
return ERROR;
}
static int
bma180_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int result = ERROR;
switch (cmd) {
case BMA180_SETRATE:
result = bma180_set_rate(arg);
break;
case BMA180_SETRANGE:
result = bma180_set_range(arg);
break;
case BMA180_SETBUFFER:
bma180_dev.buffer = (struct bma180_buffer *)arg;
result = 0;
break;
}
if (result)
errno = EINVAL;
return result;
}
int
bma180_attach(struct spi_dev_s *spi, int spi_id)
{
int result = ERROR;
bma180_dev.spi = spi;
bma180_dev.spi_id = spi_id;
SPI_LOCK(bma180_dev.spi, true);
/* verify that the device is attached and functioning */
if (bma180_read_reg(ADDR_CHIP_ID) == CHIP_ID) {
bma180_write_reg(ADDR_RESET, SOFT_RESET); // page 48
up_udelay(13000); // wait 12 ms, see page 49
/* Configuring the BMA180 */
/* enable writing to chip config */
uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 |= REG0_WRITE_ENABLE;
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
/* disable I2C interface, datasheet page 31 */
uint8_t disi2c = bma180_read_reg(ADDR_DIS_I2C);
disi2c |= 0x01;
bma180_write_reg(ADDR_DIS_I2C, disi2c);
/* block writing to chip config */
ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 &= (~REG0_WRITE_ENABLE);
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
// up_udelay(500);
/* set rate */
result = bma180_set_rate(BMA180_RATE_LP_600HZ);
// up_udelay(500);
/* set range */
result += bma180_set_range(BMA180_RANGE_4G);
// up_udelay(500);
if (result == 0) {
/* make ourselves available */
register_driver("/dev/bma180", &bma180_fops, 0666, NULL);
}
} else {
errno = EIO;
}
SPI_LOCK(bma180_dev.spi, false);
return result;
}
nuttx/configs/px4fmu/src/drv_hmc5833l.c
-386
View File
@@ -1,386 +0,0 @@
/*
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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 drv_hmc5883l.c
* Driver for the Honeywell/ST HMC5883L MEMS magnetometer
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/i2c.h>
#include <arch/board/board.h>
#include "chip.h"
#include "px4fmu-internal.h"
#include <arch/board/drv_hmc5883l.h>
#define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01
#define ADDR_MODE 0x02
#define ADDR_DATA_OUT_X_MSB 0x03
#define ADDR_DATA_OUT_X_LSB 0x04
#define ADDR_DATA_OUT_Z_MSB 0x05
#define ADDR_DATA_OUT_Z_LSB 0x06
#define ADDR_DATA_OUT_Y_MSB 0x07
#define ADDR_DATA_OUT_Y_LSB 0x08
#define ADDR_STATUS 0x09
#define ADDR_ID_A 0x10
#define ADDR_ID_B 0x11
#define ADDR_ID_C 0x12
#define HMC5883L_ADDRESS 0x1E
/* modes not changeable outside of driver */
#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */
#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */
#define HMC5883L_AVERAGING_2 (1 << 5)
#define HMC5883L_AVERAGING_4 (2 << 5)
#define HMC5883L_AVERAGING_8 (3 << 5)
#define MODE_REG_CONTINOUS_MODE (0 << 0)
#define MODE_REG_SINGLE_MODE (1 << 0) /* default */
#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */
#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */
#define ID_A_WHO_AM_I 'H'
#define ID_B_WHO_AM_I '4'
#define ID_C_WHO_AM_I '3'
static FAR struct hmc5883l_dev_s hmc5883l_dev;
static ssize_t hmc5883l_read(struct file *filp, FAR char *buffer, size_t buflen);
static int hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations hmc5883l_fops = {
.open = 0,
.close = 0,
.read = hmc5883l_read,
.write = 0,
.seek = 0,
.ioctl = hmc5883l_ioctl,
#ifndef CONFIG_DISABLE_POLL
.poll = 0
#endif
};
struct hmc5883l_dev_s
{
struct i2c_dev_s *i2c;
uint8_t rate;
struct hmc5883l_buffer *buffer;
};
static bool hmc5883l_calibration_enabled = false;
static int hmc5883l_write_reg(uint8_t address, uint8_t data);
static int hmc5883l_read_reg(uint8_t address);
static int hmc5883l_reset(void);
static int
hmc5883l_write_reg(uint8_t address, uint8_t data)
{
uint8_t cmd[] = {address, data};
return I2C_WRITE(hmc5883l_dev.i2c, cmd, 2);
}
static int
hmc5883l_read_reg(uint8_t address)
{
uint8_t cmd = address;
uint8_t data;
int ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, &data, 1);
/* return data on success, error code on failure */
if (ret == OK) {
ret = data;
}
return ret;
}
static int
hmc5883l_set_range(uint8_t range)
{
I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
/* mask out illegal bit positions */
uint8_t write_range = range; //& REG4_RANGE_MASK;
/* immediately return if user supplied invalid value */
if (write_range != range) return EINVAL;
/* set remaining bits to a sane value */
// write_range |= REG4_BDU;
/* write to device */
hmc5883l_write_reg(ADDR_CONF_B, write_range);
/* return 0 if register value is now written value, 1 if unchanged */
return !(hmc5883l_read_reg(ADDR_CONF_B) == write_range);
}
static int
hmc5883l_set_rate(uint8_t rate)
{
I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
/* mask out illegal bit positions */
uint8_t write_rate = rate;// & REG1_RATE_LP_MASK;
/* immediately return if user supplied invalid value */
if (write_rate != rate) return EINVAL;
/* set remaining bits to a sane value */
// write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
write_rate |= HMC5883L_AVERAGING_8;
/* write to device */
hmc5883l_write_reg(ADDR_CONF_A, write_rate);
/* return 0 if register value is now written value, 1 if unchanged */
return !(hmc5883l_read_reg(ADDR_CONF_A) == write_rate);
}
static int
hmc5883l_set_mode(uint8_t mode)
{
// I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
// /* mask out illegal bit positions */
// uint8_t write_mode = mode & 0x03;
// /* immediately return if user supplied invalid value */
// if (write_mode != mode) return EINVAL;
// /* set mode */
// write_mode |= hmc5883l_read_reg(ADDR_CONF_A);
// /* set remaining bits to a sane value */
// write_mode |= HMC5883L_AVERAGING_8;
// /* write to device */
// hmc5883l_write_reg(ADDR_CONF_A, write_mode);
// /* return 0 if register value is now written value, 1 if unchanged */
// return !(hmc5883l_read_reg(ADDR_CONF_A) == write_mode);
}
static bool
read_values(int16_t *data)
{
struct { /* status register and data as read back from the device */
int16_t x;
int16_t z;
int16_t y;
uint8_t status;
} __attribute__((packed)) hmc_report;
hmc_report.status = 0;
static int read_err_count = 0;
/* exchange the report structure with the device */
uint8_t cmd = ADDR_DATA_OUT_X_MSB;
int ret = 0;
I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
/* set device into single mode, trigger next measurement */
ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
/* Only execute consecutive steps on success */
if (ret == OK)
{
cmd = ADDR_DATA_OUT_X_MSB;
ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, (uint8_t*)&hmc_report, 6);
if (ret == OK)
{
/* Six bytes to read, stop if timed out */
int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
if (hmc_status < 0)
{
//if (hmc_status == ETIMEDOUT)
hmc5883l_reset();
ret = hmc_status;
}
else
{
hmc_report.status = hmc_status;
ret = OK;
}
}
else
{
if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset();
}
}
else
{
if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset();
}
if (ret != OK)
{
read_err_count++;
/* If the last reads failed as well, reset the bus and chip */
if (read_err_count > 3) hmc5883l_reset();
*get_errno_ptr() = -ret;
} else {
read_err_count = 0;
/* write values, and exchange the two 8bit blocks (big endian to little endian) */
data[0] = ((hmc_report.x & 0x00FF) << 8) | ((hmc_report.x & 0xFF00) >> 8);
data[1] = ((hmc_report.y & 0x00FF) << 8) | ((hmc_report.y & 0xFF00) >> 8);
data[2] = ((hmc_report.z & 0x00FF) << 8) | ((hmc_report.z & 0xFF00) >> 8);
// XXX TODO
// write mode, range and lp-frequency enum values into data[3]-[6]
if ((hmc_report.status & STATUS_REG_DATA_READY) > 0)
{
ret = 14;
} else {
ret = -EAGAIN;
}
}
/* return len if new data is available, error else. hmc_report.status is 0 on errors */
return ret;
}
static ssize_t
hmc5883l_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 14) {
return read_values((int16_t *)buffer);
}
/* buffer too small */
*get_errno_ptr() = ENOSPC;
return -ERROR;
}
static int
hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int result = ERROR;
switch (cmd) {
case HMC5883L_SETRATE:
result = hmc5883l_set_rate(arg);
break;
case HMC5883L_SETRANGE:
result = hmc5883l_set_range(arg);
break;
case HMC5883L_CALIBRATION_ON:
hmc5883l_calibration_enabled = true;
result = OK;
break;
case HMC5883L_CALIBRATION_OFF:
hmc5883l_calibration_enabled = false;
result = OK;
break;
//
// case HMC5883L_SETBUFFER:
// hmc5883l_dev.buffer = (struct hmc5883l_buffer *)arg;
// result = 0;
// break;
case HMC5883L_RESET:
result = hmc5883l_reset();
break;
}
if (result)
errno = EINVAL;
return result;
}
extern int up_i2creset(FAR struct i2c_dev_s * dev);
int hmc5883l_reset()
{
int ret;
#if 1
ret = up_i2creset(hmc5883l_dev.i2c);
printf("HMC5883: BUS RESET %s\n", ret ? "FAIL" : "OK");
#else
printf("[hmc5883l drv] Resettet I2C2 BUS\n");
up_i2cuninitialize(hmc5883l_dev.i2c);
hmc5883l_dev.i2c = up_i2cinitialize(2);
I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000);
#endif
return ret;
}
int
hmc5883l_attach(struct i2c_dev_s *i2c)
{
int result = ERROR;
hmc5883l_dev.i2c = i2c;
// I2C_LOCK(hmc5883l_dev.i2c, true);
I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
uint8_t cmd = ADDR_STATUS;
uint8_t status_id[4] = {0, 0, 0, 0};
int ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4);
/* verify that the device is attached and functioning */
if ((ret >= 0) && (status_id[1] == ID_A_WHO_AM_I) && (status_id[2] == ID_B_WHO_AM_I) && (status_id[3] == ID_C_WHO_AM_I)) {
/* set update rate to 75 Hz */
/* set 0.88 Ga range */
if ((ret != 0) || (hmc5883l_set_range(HMC5883L_RANGE_0_88GA) != 0) ||
(hmc5883l_set_rate(HMC5883L_RATE_75HZ) != 0))
{
errno = EIO;
} else {
/* set device into single mode, start measurement */
ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
/* make ourselves available */
register_driver("/dev/hmc5883l", &hmc5883l_fops, 0666, NULL);
result = 0;
}
} else {
errno = EIO;
}
return result;
}
nuttx/configs/px4fmu/src/drv_l3gd20.c
-364
View File
@@ -1,364 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST L3GD20 MEMS gyroscope
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/spi.h>
#include <arch/board/board.h>
#include <nuttx/arch.h>
#include <arch/board/drv_l3gd20.h>
#include "chip.h"
#include "stm32_internal.h"
#include "px4fmu-internal.h"
#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)
static FAR struct l3gd20_dev_s l3gd20_dev;
static ssize_t l3gd20_read(struct file *filp, FAR char *buffer, size_t buflen);
static int l3gd20_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations l3gd20_fops = {
.open = 0,
.close = 0,
.read = l3gd20_read,
.write = 0,
.seek = 0,
.ioctl = l3gd20_ioctl,
#ifndef CONFIG_DISABLE_POLL
.poll = 0
#endif
};
struct l3gd20_dev_s
{
struct spi_dev_s *spi;
int spi_id;
uint8_t rate;
struct l3gd20_buffer *buffer;
};
static void l3gd20_write_reg(uint8_t address, uint8_t data);
static uint8_t l3gd20_read_reg(uint8_t address);
static void
l3gd20_write_reg(uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, true);
SPI_SNDBLOCK(l3gd20_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, false);
}
static uint8_t
l3gd20_read_reg(uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, true);
SPI_EXCHANGE(l3gd20_dev.spi, cmd, data, sizeof(cmd));
SPI_SELECT(l3gd20_dev.spi, l3gd20_dev.spi_id, false);
return data[1];
}
static int
set_range(uint8_t range)
{
/* mask out illegal bit positions */
uint8_t write_range = range & REG4_RANGE_MASK;
/* immediately return if user supplied invalid value */
if (write_range != range) return EINVAL;
/* set remaining bits to a sane value */
write_range |= REG4_BDU;
/* write to device */
l3gd20_write_reg(ADDR_CTRL_REG4, write_range);
/* return 0 if register value is now written value, 1 if unchanged */
return !(l3gd20_read_reg(ADDR_CTRL_REG4) == write_range);
}
static int
set_rate(uint8_t rate)
{
/* mask out illegal bit positions */
uint8_t write_rate = rate & REG1_RATE_LP_MASK;
/* immediately return if user supplied invalid value */
if (write_rate != rate) return EINVAL;
/* set remaining bits to a sane value */
write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
/* write to device */
l3gd20_write_reg(ADDR_CTRL_REG1, write_rate);
/* return 0 if register value is now written value, 1 if unchanged */
return !(l3gd20_read_reg(ADDR_CTRL_REG1) == write_rate);
}
static int
read_fifo(int16_t *data)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report = {.status = 11};
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
SPI_LOCK(l3gd20_dev.spi, true);
SPI_SELECT(l3gd20_dev.spi, PX4_SPIDEV_GYRO, true);
SPI_SETFREQUENCY(l3gd20_dev.spi, 25000000);
SPI_EXCHANGE(l3gd20_dev.spi, &report, &report, sizeof(report));
/* XXX if the status value is unchanged, attempt a second exchange */
if (report.status == 11) SPI_EXCHANGE(l3gd20_dev.spi, &report, &report, sizeof(report));
/* XXX set magic error value if this still didn't succeed */
if (report.status == 11) report.status = 12;
SPI_SETFREQUENCY(l3gd20_dev.spi, 10000000);
SPI_SELECT(l3gd20_dev.spi, PX4_SPIDEV_GYRO, false);
SPI_LOCK(l3gd20_dev.spi, false);
data[0] = report.x;
data[1] = report.y;
data[2] = report.z;
/* if all axes are valid, return buflen (6), else return negative status */
int ret = -((int)report.status);
if (STATUS_ZYXDA == (report.status & STATUS_ZYXDA) || STATUS_ZYXOR == (report.status & STATUS_ZYXOR))
{
ret = 6;
}
return ret;
}
static ssize_t
l3gd20_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 6) {
/* return buflen or a negative value */
int ret = read_fifo((int16_t *)buffer);
if (ret != 6) *get_errno_ptr() = EAGAIN;
return ret;
}
/* buffer too small */
*get_errno_ptr() = ENOSPC;
return ERROR;
}
static int
l3gd20_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int result = ERROR;
switch (cmd) {
case L3GD20_SETRATE:
if ((arg & REG1_RATE_LP_MASK) == arg) {
SPI_LOCK(l3gd20_dev.spi, true);
set_rate(arg);
SPI_LOCK(l3gd20_dev.spi, false);
result = 0;
l3gd20_dev.rate = arg;
}
break;
case L3GD20_SETRANGE:
if ((arg & REG4_RANGE_MASK) == arg) {
SPI_LOCK(l3gd20_dev.spi, true);
set_range(arg);
SPI_LOCK(l3gd20_dev.spi, false);
result = 0;
}
break;
case L3GD20_SETBUFFER:
l3gd20_dev.buffer = (struct l3gd20_buffer *)arg;
result = 0;
break;
}
if (result)
errno = EINVAL;
return result;
}
int
l3gd20_attach(struct spi_dev_s *spi, int spi_id)
{
int result = ERROR;
l3gd20_dev.spi = spi;
l3gd20_dev.spi_id = spi_id;
SPI_LOCK(l3gd20_dev.spi, true);
/* read dummy value to void to clear SPI statemachine on sensor */
(void)l3gd20_read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning */
if (l3gd20_read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
/* reset device memory */
//l3gd20_write_reg(ADDR_CTRL_REG5, REG5_REBOOT_MEMORY);
//up_udelay(1000);
/* set default configuration */
l3gd20_write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
l3gd20_write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
l3gd20_write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
l3gd20_write_reg(ADDR_CTRL_REG4, 0x10);
l3gd20_write_reg(ADDR_CTRL_REG5, 0);
l3gd20_write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
l3gd20_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)) /* takes device out of low-power mode */
{
errno = EIO;
} else {
/* Read out the first few funky values */
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
up_udelay(500);
/* And read another set */
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
/* make ourselves available */
register_driver("/dev/l3gd20", &l3gd20_fops, 0666, NULL);
result = 0;
}
} else {
errno = EIO;
}
SPI_LOCK(l3gd20_dev.spi, false);
return result;
}
nuttx/configs/px4fmu/src/drv_lis331.c
-272
View File
@@ -1,272 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* Driver for the ST LIS331 MEMS accelerometer
*/
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/spi.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "chip.h"
#include "stm32_internal.h"
#include "px4fmu-internal.h"
#include <arch/board/drv_lis331.h>
/*
* LIS331 registers
*/
#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 0x32
#define ADDR_CTRL_REG1 0x20 /* sample rate constants are in the public header */
#define REG1_POWER_NORMAL (1<<5)
#define REG1_RATE_MASK (3<<3)
#define REG1_Z_ENABLE (1<<2)
#define REG1_Y_ENABLE (1<<1)
#define REG1_X_ENABLE (1<<0)
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
#define ADDR_CTRL_REG4 0x23
#define REG4_BDU (1<<7)
#define REG4_BIG_ENDIAN (1<<6)
#define REG4_RANGE_MASK (3<<4)
#define REG4_SPI_3WIRE (1<<0)
#define ADDR_CTRL_REG5 0x24
#define ADDR_HP_FILTER_RESET 0x25
#define ADDR_REFERENCE 0x26
#define ADDR_STATUS_REG 0x27
#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 ADDR_OUT_X 0x28 /* 16 bits */
#define ADDR_OUT_Y 0x2A /* 16 bits */
#define ADDR_OUT_Z 0x2C /* 16 bits */
static ssize_t lis331_read(struct file *filp, FAR char *buffer, size_t buflen);
static int lis331_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations lis331_fops = {
.read = lis331_read,
.ioctl = lis331_ioctl,
};
struct lis331_dev_s
{
struct spi_dev_s *spi;
int spi_id;
uint8_t rate;
struct lis331_buffer *buffer;
};
static struct lis331_dev_s lis331_dev;
static void write_reg(uint8_t address, uint8_t data);
static uint8_t read_reg(uint8_t address);
static bool read_fifo(uint16_t *data);
static void set_range(uint8_t range);
static void set_rate(uint8_t rate);
static void
write_reg(uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
SPI_SNDBLOCK(lis331_dev.spi, &cmd, sizeof(cmd));
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
}
static uint8_t
read_reg(uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
SPI_EXCHANGE(lis331_dev.spi, cmd, data, sizeof(cmd));
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
return data[1];
}
static bool
read_fifo(uint16_t *data)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = ADDR_STATUS_REG | DIR_READ | ADDR_INCREMENT;
/* exchange the report structure with the device */
SPI_LOCK(lis331_dev.spi, true);
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
SPI_EXCHANGE(lis331_dev.spi, &report, &report, sizeof(report));
SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
SPI_LOCK(lis331_dev.spi, false);
data[0] = report.x;
data[1] = report.y;
data[2] = report.z;
return report.status & STATUS_ZYXDA;
}
static void
set_range(uint8_t range)
{
range &= REG4_RANGE_MASK;
write_reg(ADDR_CTRL_REG4, range | REG4_BDU);
}
static void
set_rate(uint8_t rate)
{
rate &= REG1_RATE_MASK;
write_reg(ADDR_CTRL_REG1, rate | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
}
static ssize_t
lis331_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 12) {
if (read_fifo((uint16_t *)buffer))
return 12;
/* no data */
return 0;
}
/* buffer too small */
errno = ENOSPC;
return ERROR;
}
static int
lis331_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int result = ERROR;
switch (cmd) {
case LIS331_SETRATE:
if ((arg & REG1_RATE_MASK) == arg) {
set_rate(arg);
result = 0;
lis331_dev.rate = arg;
}
break;
case LIS331_SETRANGE:
if ((arg & REG4_RANGE_MASK) == arg) {
set_range(arg);
result = 0;
}
break;
case LIS331_SETBUFFER:
lis331_dev.buffer = (struct lis331_buffer *)arg;
result = 0;
break;
}
if (result)
errno = EINVAL;
return result;
}
int
lis331_attach(struct spi_dev_s *spi, int spi_id)
{
int result = ERROR;
lis331_dev.spi = spi;
SPI_LOCK(lis331_dev.spi, true);
/* verify that the device is attached and functioning */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
/* set default configuration */
write_reg(ADDR_CTRL_REG2, 0); /* disable interrupt-generating high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG5, 0); /* disable wake-on-interrupt */
set_range(LIS331_RANGE_4G);
set_rate(LIS331_RATE_400Hz); /* takes device out of low-power mode */
/* make ourselves available */
register_driver("/dev/lis331", &lis331_fops, 0666, NULL);
result = 0;
} else {
errno = EIO;
}
SPI_LOCK(lis331_dev.spi, false);
return result;
}
nuttx/configs/px4fmu/src/drv_ms5611.c
-504
View File
File diff suppressed because it is too large Load Diff
nuttx/configs/px4fmu/src/up_nsh.c
+1 -30
View File
@@ -61,8 +61,6 @@
#include <arch/board/drv_tone_alarm.h>
#include <arch/board/up_adc.h>
#include <arch/board/board.h>
#include <arch/board/drv_bma180.h>
#include <arch/board/drv_l3gd20.h>
#include <arch/board/drv_led.h>
#include <arch/board/drv_eeprom.h>
@@ -181,7 +179,7 @@ int nsh_archinitialize(void)
return -ENODEV;
}
// Setup 10 MHz clock (maximum rate the BMA180 can sustain)
// Default SPI1 to 1MHz and de-assert the known chip selects.
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
@@ -192,33 +190,6 @@ int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
/* initialize SPI peripherals redundantly */
int gyro_attempts = 0;
int gyro_fail = 0;
while (gyro_attempts < 5)
{
gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO);
gyro_attempts++;
if (gyro_fail == 0) break;
up_udelay(1000);
}
if (!gyro_fail) message("[boot] Found L3GD20 gyro\n");
int acc_attempts = 0;
int acc_fail = 0;
while (acc_attempts < 5)
{
acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL);
acc_attempts++;
if (acc_fail == 0) break;
up_udelay(1000);
}
if (!acc_fail) message("[boot] Found BMA180 accelerometer\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);