diff options
-rw-r--r-- | apps/drivers/device/cdev.cpp | 4 | ||||
-rw-r--r-- | apps/drivers/device/device.h | 6 | ||||
-rw-r--r-- | apps/px4/fmu/fmu.cpp | 51 | ||||
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 549 | ||||
-rw-r--r-- | apps/px4/px4io/driver/uploader.cpp | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_sensors.c | 330 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_bma180.h | 99 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_hmc5883l.h | 100 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_l3gd20.h | 108 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_lis331.h | 83 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_ms5611.h | 76 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/Makefile | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_bma180.c | 341 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_hmc5833l.c | 386 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_l3gd20.c | 364 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_lis331.c | 272 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_ms5611.c | 504 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_nsh.c | 31 |
19 files changed, 345 insertions, 2965 deletions
diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp index 6b8bf0c2d..d07d26e82 100644 --- a/apps/drivers/device/cdev.cpp +++ b/apps/drivers/device/cdev.cpp @@ -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; diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h index 98dbf8bfb..9af678465 100644 --- a/apps/drivers/device/device.h +++ b/apps/drivers/device/device.h @@ -287,6 +287,12 @@ public: 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. * diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 18f27d49e..6d990c46b 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -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; diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index c3371c138..66db1c360 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -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,185 +61,122 @@ #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 +class PX4IO : public device::CDev { public: - PX4IO_RC(); - ~PX4IO_RC(); + PX4IO(); + ~PX4IO(); 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); + virtual int ioctl(file *filp, int cmd, unsigned long arg); private: - orb_advert_t _publication; - struct rc_input_values _input; -}; + static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; -/* 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; -} + int _serial_fd; ///< serial interface to PX4IO + hx_stream_t _io_stream; ///< HX protocol stream -PX4IO_RC::~PX4IO_RC() -{ - if (_publication != -1) - ::close(_publication); -} + int _task; ///< worker task + volatile bool _task_should_exit; -int -PX4IO_RC::init() -{ - int ret; + int _t_actuators; ///< actuator output topic + actuator_controls_s _controls; ///< actuator outputs - ret = CDev::init(); + int _t_armed; ///< system armed control topic + actuator_armed_s _armed; ///< system armed state - /* advertise ourselves as the RC input controller */ - if (ret == OK) { - _publication = orb_advertise(ORB_ID(input_rc), &_input); - if (_publication < 0) - ret = -errno; - } + orb_advert_t _t_outputs; ///< mixed outputs topic + actuator_outputs_s _outputs; ///< mixed outputs - return ret; -} + MixerGroup *_mixers; ///< loaded mixers -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; + bool _primary_pwm_device; ///< true if we are the default PWM output - if (channels > PX4IO_INPUT_CHANNELS) - return -EIO; + volatile bool _switch_armed; ///< PX4IO switch armed state + // XXX how should this work? - lock(); - for (i = 0; i < channels; i++) - pdata[i] = _input.values[i]; - unlock(); + bool _send_needed; ///< If true, we need to send a packet to IO - return i * sizeof(servo_position_t); -} + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(int argc, char *argv[]); -void -PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) -{ + /** + * worker task + */ + void task_main(); - ASSERT(count <= PX4IO_INPUT_CHANNELS); + /** + * Handle receiving bytes from PX4IO + */ + void io_recv(); - /* convert incoming servo position values into 0-100 range */ - lock(); - for (unsigned i = 0; i < count; i++) { - rc_input_t chn; + /** + * HX protocol callback trampoline. + */ + static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); - if (data[i] < 1000) { - chn = 0; - } else if (data[i] > 2000) { - chn = 100; - } else { - chn = (data[i] - 1000) / 10; - } + /** + * Callback invoked when we receive a whole packet from PX4IO + */ + void rx_callback(const uint8_t *buffer, size_t bytes_received); - _input.values[i] = chn; - } - _input.channel_count = count; - unlock(); + /** + * Send an update packet to PX4IO + */ + void io_send(); - /* publish to anyone that might be listening */ - if (_publication != -1) - orb_publish(ORB_ID(input_rc), _publication, &_input); + /** + * 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); +}; -} -class PX4IO : public device::CDev +namespace { -public: - PX4IO(); - ~PX4IO(); - - 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); - -private: - int _fd; - int _task; - PX4IO_RC *_rc; - - /** command to be sent to IO */ - struct px4io_command _next_command; - /** RC channel input from IO */ - servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; - int _rc_channel_count; - - volatile bool _armed; - volatile bool _task_should_exit; - - bool _send_needed; - - hx_stream_t _io_stream; - - static void task_main_trampoline(int argc, char *argv[]); - void task_main(); - void io_recv(); - - static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); - void rx_callback(const uint8_t *buffer, size_t bytes_received); +PX4IO *g_dev; - void io_send(); -}; +} 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); + + /* XXX send a who-are-you request */ + + /* XXX verify firmware/protocol version */ - /* poll descriptor(s) */ - struct pollfd fds[1]; - fds[0].fd = _fd; + /* + * 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; - - /* handle bytes from IO */ - while (::read(_fd, &c, 1) == 1) - hx_stream_rx(_io_stream, c); + uint8_t buf[32]; + int count; + + /* + * 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; - 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; + 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; - /* channel get? */ - if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { - int channel = cmd - PWM_SERVO_GET(0); + 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; - /* currently no data for this channel */ - if (channel >= _rc_channel_count) { - ret = -ERANGE; - 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); } - *(servo_position_t *)arg = _rc_channel[channel]; - ret = 0; - break; } + 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: /* 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'"); } diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp index 7d6a9e171..0fbbac839 100644 --- a/apps/px4/px4io/driver/uploader.cpp +++ b/apps/px4/px4io/driver/uploader.cpp @@ -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; } diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index 87ea7f058..c801869ab 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -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 ****************************************************************************/ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 96e0ca350..edc2c4bd2 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -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. diff --git a/nuttx/configs/px4fmu/include/drv_bma180.h b/nuttx/configs/px4fmu/include/drv_bma180.h deleted file mode 100644 index a403415e4..000000000 --- a/nuttx/configs/px4fmu/include/drv_bma180.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/include/drv_hmc5883l.h b/nuttx/configs/px4fmu/include/drv_hmc5883l.h deleted file mode 100644 index 741c3e154..000000000 --- a/nuttx/configs/px4fmu/include/drv_hmc5883l.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/include/drv_l3gd20.h b/nuttx/configs/px4fmu/include/drv_l3gd20.h deleted file mode 100644 index 3b284d60d..000000000 --- a/nuttx/configs/px4fmu/include/drv_l3gd20.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/include/drv_lis331.h b/nuttx/configs/px4fmu/include/drv_lis331.h deleted file mode 100644 index f4699cda0..000000000 --- a/nuttx/configs/px4fmu/include/drv_lis331.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/include/drv_ms5611.h b/nuttx/configs/px4fmu/include/drv_ms5611.h deleted file mode 100644 index 922a11219..000000000 --- a/nuttx/configs/px4fmu/include/drv_ms5611.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 5a60b7d19..48b528f6a 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -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 diff --git a/nuttx/configs/px4fmu/src/drv_bma180.c b/nuttx/configs/px4fmu/src/drv_bma180.c deleted file mode 100644 index da80cc2e2..000000000 --- a/nuttx/configs/px4fmu/src/drv_bma180.c +++ /dev/null @@ -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; -} - diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c deleted file mode 100644 index d51d654f7..000000000 --- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c +++ /dev/null @@ -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; -} diff --git a/nuttx/configs/px4fmu/src/drv_l3gd20.c b/nuttx/configs/px4fmu/src/drv_l3gd20.c deleted file mode 100644 index 19f2d032f..000000000 --- a/nuttx/configs/px4fmu/src/drv_l3gd20.c +++ /dev/null @@ -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; -} diff --git a/nuttx/configs/px4fmu/src/drv_lis331.c b/nuttx/configs/px4fmu/src/drv_lis331.c deleted file mode 100644 index fd477b46f..000000000 --- a/nuttx/configs/px4fmu/src/drv_lis331.c +++ /dev/null @@ -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; -} - diff --git a/nuttx/configs/px4fmu/src/drv_ms5611.c b/nuttx/configs/px4fmu/src/drv_ms5611.c deleted file mode 100644 index c7e91c5ea..000000000 --- a/nuttx/configs/px4fmu/src/drv_ms5611.c +++ /dev/null @@ -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; -} diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index 25f3c2795..af8253181 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -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); |