fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 05:43:33 +08:00
Code Issues 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
@@ -37,8 +37,7 @@
*
* PX4IO is connected via serial (or possibly some other interface at a later
* point).
*
* XXX current design is racy as all hell; need a locking strategy.
*/
#include <nuttx/config.h>
@@ -53,6 +52,7 @@
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
@@ -61,129 +61,21 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
#include "px4io/protocol.h"
#include "uploader.h"
class PX4IO;
namespace
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
PX4IO *g_dev;
}
class PX4IO_RC : public device::CDev
{
public:
PX4IO_RC();
~PX4IO_RC();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
friend class PX4IO;
protected:
void set_channels(unsigned count, const servo_position_t *data);
private:
orb_advert_t _publication;
struct rc_input_values _input;
};
/* XXX this may conflict with the onboard PPM input */
PX4IO_RC::PX4IO_RC() :
CDev("px4io_rc", RC_INPUT_DEVICE_PATH),
_publication(-1)
{
for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
_input.values[i] = 0;
}
_input.channel_count = 0;
}
PX4IO_RC::~PX4IO_RC()
{
if (_publication != -1)
::close(_publication);
}
int
PX4IO_RC::init()
{
int ret;
ret = CDev::init();
/* advertise ourselves as the RC input controller */
if (ret == OK) {
_publication = orb_advertise(ORB_ID(input_rc), &_input);
if (_publication < 0)
ret = -errno;
}
return ret;
}
ssize_t
PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned channels = buflen / sizeof(rc_input_t);
rc_input_t *pdata = (rc_input_t *)buffer;
unsigned i;
if (channels > PX4IO_INPUT_CHANNELS)
return -EIO;
lock();
for (i = 0; i < channels; i++)
pdata[i] = _input.values[i];
unlock();
return i * sizeof(servo_position_t);
}
void
PX4IO_RC::set_channels(unsigned count, const servo_position_t *data)
{
ASSERT(count <= PX4IO_INPUT_CHANNELS);
/* convert incoming servo position values into 0-100 range */
lock();
for (unsigned i = 0; i < count; i++) {
rc_input_t chn;
if (data[i] < 1000) {
chn = 0;
} else if (data[i] > 2000) {
chn = 100;
} else {
chn = (data[i] - 1000) / 10;
}
_input.values[i] = chn;
}
_input.channel_count = count;
unlock();
/* publish to anyone that might be listening */
if (_publication != -1)
orb_publish(ORB_ID(input_rc), _publication, &_input);
}
class PX4IO : public device::CDev
{
@@ -193,53 +85,98 @@ public:
virtual int init();
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
private:
int _fd;
int _task;
PX4IO_RC *_rc;
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
/** command to be sent to IO */
struct px4io_command _next_command;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
/** RC channel input from IO */
servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS];
int _rc_channel_count;
volatile bool _armed;
int _task; ///< worker task
volatile bool _task_should_exit;
bool _send_needed;
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
hx_stream_t _io_stream;
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
bool _primary_pwm_device; ///< true if we are the default PWM output
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
bool _send_needed; ///< If true, we need to send a packet to IO
/**
* Trampoline to the worker task
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* worker task
*/
void task_main();
/**
* Handle receiving bytes from PX4IO
*/
void io_recv();
/**
* HX protocol callback trampoline.
*/
static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
/**
* Callback invoked when we receive a whole packet from PX4IO
*/
void rx_callback(const uint8_t *buffer, size_t bytes_received);
/**
* Send an update packet to PX4IO
*/
void io_send();
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
*/
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
};
namespace
{
PX4IO *g_dev;
}
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
_fd(-1),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_rc(new PX4IO_RC),
_rc_channel_count(0),
_armed(false),
_task_should_exit(false),
_send_needed(false),
_io_stream(nullptr)
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false)
{
/* set up the command we will use */
_next_command.f2i_magic = F2I_MAGIC;
/* we need this potentially before it could be set in px4io_main */
/* we need this potentially before it could be set in task_main */
g_dev = this;
_debug_enabled = true;
@@ -247,19 +184,18 @@ PX4IO::PX4IO() :
PX4IO::~PX4IO()
{
if (_rc != nullptr)
delete _rc;
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;
/* spin waiting for the thread to stop */
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;
}
@@ -267,6 +203,14 @@ PX4IO::~PX4IO()
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
g_dev = nullptr;
}
@@ -277,17 +221,20 @@ PX4IO::init()
ASSERT(_task == -1);
/* XXX send a who-are-you request */
/* XXX verify firmware/protocol version */
/* do regular cdev init */
ret = CDev::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_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -305,36 +252,61 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
ASSERT(_fd == -1);
log("ready");
log("starting");
/* open the serial port */
_fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK);
if (_fd < 0) {
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
if (_serial_fd < 0) {
debug("failed to open serial port for IO: %d", errno);
_task = -1;
_exit(errno);
}
/* protocol stream */
_io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this);
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors);
hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors);
/* poll descriptor(s) */
struct pollfd fds[1];
fds[0].fd = _fd;
/* XXX send a who-are-you request */
/* XXX verify firmware/protocol version */
/*
* 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, 20); /* XXX 50Hz hardcoded for now */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
log("ready");
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], 1, 100);
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000);
/* this would be bad... */
if (ret < 0) {
@@ -347,9 +319,36 @@ PX4IO::task_main()
if (ret == 0)
_send_needed = true;
/* if we have new data from IO, go handle it */
if ((ret > 0) && (fds[0].revents & POLLIN))
io_recv();
if (ret > 0) {
/* if we have new data from IO, go handle it */
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
}
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
_send_needed = true;
}
}
/* send an update to IO if required */
if (_send_needed) {
@@ -357,23 +356,40 @@ PX4IO::task_main()
io_send();
}
}
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
int
PX4IO::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
return 0;
}
void
PX4IO::io_recv()
{
uint8_t c;
uint8_t buf[32];
int count;
/* handle bytes from IO */
while (::read(_fd, &c, 1) == 1)
hx_stream_rx(_io_stream, c);
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
count = ::read(_serial_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++)
hx_stream_rx(_io_stream, buf[i]);
}
void
@@ -385,98 +401,139 @@ PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_receiv
void
PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const struct px4io_report *rep = (const struct px4io_report *)buffer;
const px4io_report *rep = (const px4io_report *)buffer;
/* sanity-check the received frame size */
if (bytes_received != sizeof(struct px4io_report))
if (bytes_received != sizeof(px4io_report))
return;
lock();
/* XXX handle R/C inputs here ... needs code sharing/library */
/* pass RC input data to the driver */
if (_rc != nullptr)
_rc->set_channels(rep->channel_count, &rep->rc_channel[0]);
_armed = rep->armed;
/* send an update frame */
_send_needed = true;
/* remember the latched arming switch state */
_switch_armed = rep->armed;
unlock();
}
void
PX4IO::io_send()
{
lock();
px4io_command cmd;
/* send packet to IO while we're guaranteed it won't change */
hx_stream_send(_io_stream, &_next_command, sizeof(_next_command));
cmd.f2i_magic = F2I_MAGIC;
unlock();
}
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
ssize_t
PX4IO::write(struct file *filp, const char *buffer, size_t len)
{
unsigned channels = len / sizeof(servo_position_t);
servo_position_t *pdata = (servo_position_t *)buffer;
unsigned i;
/* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
if (channels > PX4IO_OUTPUT_CHANNELS)
return -EIO;
// XXX relays
lock();
for (i = 0; i < channels; i++)
_next_command.servo_command[i] = pdata[i];
unlock();
cmd.arm_ok = _armed.armed;
return i * sizeof(servo_position_t);
hx_stream_send(_io_stream, &cmd, sizeof(cmd));
}
int
PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg)
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = -ENOTTY;
int ret = OK;
lock();
/* regular ioctl? */
switch (cmd) {
case PWM_SERVO_ARM:
_next_command.arm_ok = true;
ret = 0;
/* fake an armed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_DISARM:
_next_command.arm_ok = false;
ret = 0;
/* fake a disarmed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
} else {
ret = -EINVAL;
}
break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
break;
default:
/* channel set? */
if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) {
/* XXX sanity-check value? */
_next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg;
ret = 0;
break;
}
/* channel get? */
if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) {
int channel = cmd - PWM_SERVO_GET(0);
/* currently no data for this channel */
if (channel >= _rc_channel_count) {
ret = -ERANGE;
break;
}
*(servo_position_t *)arg = _rc_channel[channel];
ret = 0;
break;
}
/* not a recognised value */
ret = -ENOTTY;
}
@@ -492,28 +549,25 @@ px4io_main(int argc, char *argv[])
{
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "PX4IO: already loaded\n");
return -EBUSY;
}
if (g_dev != nullptr)
errx(1, "already loaded");
/* create the driver - it will set g_dev */
(void)new PX4IO;
if (g_dev == nullptr) {
fprintf(stderr, "PX4IO: driver alloc failed\n");
return -ENOMEM;
}
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
fprintf(stderr, "PX4IO: driver init failed\n");
delete g_dev;
return -EIO;
errx(1, "driver init failed");
}
return OK;
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
PX4IO_Uploader *up;
const char *fn[3];
@@ -536,26 +590,19 @@ px4io_main(int argc, char *argv[])
case OK:
break;
case -ENOENT:
fprintf(stderr, "PX4IO firmware file not found\n");
break;
errx(1, "PX4IO firmware file not found");
case -EEXIST:
case -EIO:
fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n");
break;
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
case -EINVAL:
fprintf(stderr, "verify failed - retry the update\n");
break;
errx(1, "verify failed - retry the update");
case -ETIMEDOUT:
fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n");
errx(1, "timed out waiting for bootloader - power-cycle and try again");
default:
fprintf(stderr, "unexpected error %d\n", ret);
break;
errx(1, "unexpected error %d", ret);
}
return ret;
}
printf("need a verb, only support 'start' and 'update'\n");
return ERROR;
errx(1, "need a verb, only support 'start' and 'update'");
}
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
@@ -1,504 +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 Measurement Specialties MS5611 barometric pressure sensor
*/
#include <nuttx/config.h>
#include <nuttx/i2c.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "chip.h"
#include "px4fmu-internal.h"
#include <arch/board/up_hrt.h>
#include <arch/board/drv_ms5611.h>
/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
#define MS5611_MIN_INTER_MEASUREMENT_INTERVAL 9200
#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */
#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */
#define ADDR_RESET_CMD 0x1E /* read from this address to reset chip (0b0011110 on bus) */
#define ADDR_CMD_CONVERT_D1 0x48 /* 4096 samples to this address to start conversion (0b01001000 on bus) */
#define ADDR_CMD_CONVERT_D2 0x58 /* 4096 samples */
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
static FAR struct ms5611_dev_s ms5611_dev;
static ssize_t ms5611_read(struct file *filp, FAR char *buffer, size_t buflen);
static int ms5611_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations ms5611_fops = {
.read = ms5611_read,
.ioctl = ms5611_ioctl,
};
struct ms5611_prom_s
{
uint16_t factory_setup;
uint16_t c1_pressure_sens;
uint16_t c2_pressure_offset;
uint16_t c3_temp_coeff_pres_sens;
uint16_t c4_temp_coeff_pres_offset;
uint16_t c5_reference_temp;
uint16_t c6_temp_coeff_temp;
uint16_t serial_and_crc;
} __attribute__((packed));
union ms5611_prom_u
{
uint16_t c[8];
struct ms5611_prom_s s;
} __attribute__((packed));
struct ms5611_dev_s
{
union ms5611_prom_u prom;
struct i2c_dev_s *i2c;
struct ms5611_buffer *buffer;
} __attribute__((packed));
static FAR uint8_t MS5611_ADDRESS;
static FAR struct {
/* status register and data as read back from the device */
float pressure;
float altitude;
float temperature;
uint32_t d1_raw;
uint32_t d2_raw;
uint32_t measurements_count;
uint8_t last_state;
uint64_t last_read;
} ms5611_report = {
.pressure = 0.0f,
.altitude = 0.0f,
.temperature = 0.0f,
.last_state = 0,
/* make sure the first readout can be performed */
.last_read = 0,
};
static int ms5611_read_prom(void);
int ms5611_reset()
{
int ret;
printf("[ms5611 drv] Resettet I2C2 BUS\n");
up_i2cuninitialize(ms5611_dev.i2c);
ms5611_dev.i2c = up_i2cinitialize(2);
I2C_SETFREQUENCY(ms5611_dev.i2c, 400000);
return ret;
}
static bool
read_values(float *data)
{
int ret;
uint8_t cmd_data[3];
/* check validity of pointer */
if (data == NULL)
{
*get_errno_ptr() = EINVAL;
return -EINVAL;
}
/* only start reading when data is available */
if (ms5611_report.measurements_count > 0)
{
/* do not read more often than at minimum 9.17 ms intervals */
if ((hrt_absolute_time() - ms5611_report.last_read) < MS5611_MIN_INTER_MEASUREMENT_INTERVAL)
{
/* set errno to 'come back later' */
ret = -EAGAIN;
goto handle_return;
}
else
{
/* set new value */
ms5611_report.last_read = hrt_absolute_time();
}
/* Read out last measurement */
cmd_data[0] = 0x00;
struct i2c_msg_s msgv[2] = {
{
.addr = MS5611_ADDRESS,
.flags = 0,
.buffer = cmd_data,
.length = 1
},
{
.addr = MS5611_ADDRESS,
.flags = I2C_M_READ,
.buffer = cmd_data,
.length = 3
}
};
ret = I2C_TRANSFER(ms5611_dev.i2c, msgv, 2);
if (ret != OK) goto handle_return;
/* at value 1 the last reading was temperature */
if (ms5611_report.last_state == 1)
{
/* put temperature into the raw set */
ms5611_report.d2_raw = (((uint32_t)cmd_data[0]) << 16) | (((uint32_t)cmd_data[1]) << 8) | ((uint32_t)cmd_data[2]);
}
else
{
/* put altitude into the raw set */
ms5611_report.d1_raw = (((uint32_t)cmd_data[0]) << 16) | (((uint32_t)cmd_data[1]) << 8) | ((uint32_t)cmd_data[2]);
}
}
ms5611_report.measurements_count++;
/*
* this block reads four pressure values and one temp value,
* resulting in 80 Hz pressure update and 20 Hz temperature updates
* at 100 Hz continuous operation.
*/
if (ms5611_report.last_state == 0)
{
/* request first a temperature reading */
cmd_data[0] = ADDR_CMD_CONVERT_D2;
}
else
{
/* request pressure reading */
cmd_data[0] = ADDR_CMD_CONVERT_D1;
}
if (ms5611_report.last_state == 3)
{
ms5611_report.last_state = 0;
}
else
{
ms5611_report.last_state++;
}
/* write measurement command */
struct i2c_msg_s conv_cmd[1] = {
{
.addr = MS5611_ADDRESS,
.flags = 0,
.buffer = cmd_data,
.length = 1
},
};
ret = I2C_TRANSFER(ms5611_dev.i2c, conv_cmd, 1);
if (ret != OK) goto handle_return;
/* only write back values after first complete set */
if (ms5611_report.measurements_count > 2)
{
/* Calculate results */
/* temperature calculation */
int32_t dT = ms5611_report.d2_raw - (((int32_t)ms5611_dev.prom.s.c5_reference_temp)*256);
int64_t temp_int64 = 2000 + (((int64_t)dT)*ms5611_dev.prom.s.c6_temp_coeff_temp)/8388608;
/* pressure calculation */
int64_t offset = (int64_t)ms5611_dev.prom.s.c2_pressure_offset * 65536 + ((int64_t)dT*ms5611_dev.prom.s.c4_temp_coeff_pres_offset)/128;
int64_t sens = (int64_t)ms5611_dev.prom.s.c1_pressure_sens * 32768 + ((int64_t)dT*ms5611_dev.prom.s.c3_temp_coeff_pres_sens)/256;
/* it's pretty cold, second order temperature compensation needed */
if (temp_int64 < 2000)
{
/* second order temperature compensation */
int64_t temp2 = (((int64_t)dT)*dT) >> 31;
int64_t tmp_64 = (temp_int64-2000)*(temp_int64-2000);
int64_t offset2 = (5*tmp_64)>>1;
int64_t sens2 = (5*tmp_64)>>2;
temp_int64 = temp_int64 - temp2;
offset = offset - offset2;
sens = sens - sens2;
}
int64_t press_int64 = (((ms5611_report.d1_raw*sens)/2097152-offset)/32768);
ms5611_report.temperature = temp_int64 / 100.0f;
ms5611_report.pressure = press_int64 / 100.0f;
/* convert as double for max. precision, store as float (more than enough precision) */
ms5611_report.altitude = (44330.0 * (1.0 - pow((press_int64 / 101325.0), 0.190295)));
/* Write back float values */
data[0] = ms5611_report.pressure;
data[1] = ms5611_report.altitude;
data[2] = ms5611_report.temperature;
}
else
{
/* not ready, try again */
ret = -EINPROGRESS;
}
/* return 1 if new data is available, 0 else */
handle_return:
if (ret == OK)
{
return (sizeof(ms5611_report.d1_raw) + sizeof(ms5611_report.altitude) + sizeof(ms5611_report.d2_raw));
}
else
{
errno = -ret;
if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset();
return ret;
}
}
static ssize_t
ms5611_read(struct file *filp, char *buffer, size_t buflen)
{
/* if the buffer is large enough, and data are available, return success */
if (buflen >= 12) {
return read_values((float *)buffer);
}
/* buffer too small */
errno = ENOSPC;
return -ENOSPC;
}
static int
ms5611_ioctl(struct file *filp, int cmd, unsigned long arg)
{
return -ENOSYS;
// switch (cmd) {
// case MS5611_SETRATE:
// if ((arg & REG1_RATE_LP_MASK) == arg) {
// set_rate(arg);
// result = 0;
// dev.rate = arg;
// }
// break;
//
// case MS5611_SETBUFFER:
// dev.buffer = (struct ms5611_buffer *)arg;
// result = 0;
// break;
// }
//
// if (result)
// errno = EINVAL;
// return result;
}
int ms5611_crc4(uint16_t n_prom[])
{
/* routine ported from MS5611 application note */
int16_t cnt;
uint16_t n_rem;
uint16_t crc_read;
uint8_t n_bit;
n_rem = 0x00;
/* save the read crc */
crc_read = n_prom[7];
/* remove CRC byte */
n_prom[7] = (0xFF00 & (n_prom[7]));
for (cnt = 0; cnt < 16; cnt++)
{
/* uneven bytes */
if (cnt & 1)
{
n_rem ^= (uint8_t) ((n_prom[cnt>>1]) & 0x00FF);
}
else
{
n_rem ^= (uint8_t) (n_prom[cnt>>1] >> 8);
}
for (n_bit = 8; n_bit > 0; n_bit--)
{
if (n_rem & 0x8000)
{
n_rem = (n_rem << 1) ^ 0x3000;
}
else
{
n_rem = (n_rem << 1);
}
}
}
/* final 4 bit remainder is CRC value */
n_rem = (0x000F & (n_rem >> 12));
n_prom[7] = crc_read;
/* return 0 == OK if CRCs match, 1 else */
return !((0x000F & crc_read) == (n_rem ^ 0x00));
}
int ms5611_read_prom()
{
/* read PROM data */
uint8_t prom_buf[2] = {255,255};
int retval = 0;
for (int i = 0; i < 8; i++)
{
uint8_t cmd = {ADDR_PROM_SETUP + (i*2)};
I2C_SETADDRESS(ms5611_dev.i2c, MS5611_ADDRESS, 7);
retval = I2C_WRITEREAD(ms5611_dev.i2c, &cmd, 1, prom_buf, 2);
/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
ms5611_dev.prom.c[i] = (((uint16_t)prom_buf[0])<<8) | ((uint16_t)prom_buf[1]);
if (retval != OK)
{
break;
}
}
/* calculate CRC and return error if mismatch */
return ms5611_crc4(ms5611_dev.prom.c);
}
int
ms5611_attach(struct i2c_dev_s *i2c)
{
int result = ERROR;
ms5611_dev.i2c = i2c;
MS5611_ADDRESS = MS5611_ADDRESS_1;
/* write reset command */
uint8_t cmd_data = ADDR_RESET_CMD;
struct i2c_msg_s reset_cmd[1] = {
{
.addr = MS5611_ADDRESS,
.flags = 0,
.buffer = &cmd_data,
.length = 1
},
};
int ret = I2C_TRANSFER(ms5611_dev.i2c, reset_cmd, 1);
if (ret == OK)
{
/* wait for PROM contents to be in the device (2.8 ms) */
up_udelay(3000);
/* read PROM */
ret = ms5611_read_prom();
}
/* check if the address was wrong */
if (ret != OK)
{
/* try second address */
MS5611_ADDRESS = MS5611_ADDRESS_2;
/* write reset command */
cmd_data = ADDR_RESET_CMD;
struct i2c_msg_s reset_cmd_2[1] = {
{
.addr = MS5611_ADDRESS,
.flags = 0,
.buffer = &cmd_data,
.length = 1
},
};
ret = I2C_TRANSFER(ms5611_dev.i2c, reset_cmd_2, 1);
/* wait for PROM contents to be in the device (2.8 ms) */
up_udelay(3000);
/* read PROM */
ret = ms5611_read_prom();
}
if (ret < 0) return -EIO;
/* verify that the device is attached and functioning */
if (ret == OK) {
if (MS5611_ADDRESS == MS5611_ADDRESS_1)
{
printf("[ms5611 driver] Attached MS5611 at addr #1 (0x76)\n");
}
else
{
printf("[ms5611 driver] Attached MS5611 at addr #2 (0x77)\n");
}
/* trigger temperature read */
(void)read_values(NULL);
/* wait for conversion to complete */
up_udelay(9200);
/* trigger pressure read */
(void)read_values(NULL);
/* wait for conversion to complete */
up_udelay(9200);
/* now a read_values call would obtain valid results */
/* make ourselves available */
register_driver("/dev/ms5611", &ms5611_fops, 0666, NULL);
result = OK;
} else {
errno = EIO;
}
return result;
}
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);