From af1af1e22ddd4bcd55fe9eaaf98f4a640329a4c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 18:38:30 +0200 Subject: Port expander driver first hacky version --- makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/drv_io_expander.h | 65 ++++ src/drivers/pca8574/module.mk | 6 + src/drivers/pca8574/pca8574.cpp | 569 ++++++++++++++++++++++++++++++++++ 5 files changed, 642 insertions(+) create mode 100644 src/drivers/drv_io_expander.h create mode 100644 src/drivers/pca8574/module.mk create mode 100644 src/drivers/pca8574/pca8574.cpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc..602b0af3f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl +MODULES += drivers/pca8574 # Needs to be burned to the ground and re-written; for now, diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d..0dcfd04f2 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -24,6 +24,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 +MODULES += drivers/pca8574 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h new file mode 100644 index 000000000..2705d6f9e --- /dev/null +++ b/src/drivers/drv_io_expander.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_io_expander.h + * + * IO expander device API + */ + +#pragma once + +#include +#include + +/* + * ioctl() definitions + */ + +#define _IOXIOCBASE (0x2800) +#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) + +/** enable the device */ +#define IOX_ENABLE _IOXIOC(1) + +/** set constant values */ +#define IOX_SET_VALUE _IOXIOC(2) + +/** set constant values */ +#define IOX_SET_MODE _IOXIOC(3) + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +enum IOX_MODE { + IOX_MODE_OFF, + IOX_MODE_ON +}; diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk new file mode 100644 index 000000000..825ee9bb7 --- /dev/null +++ b/src/drivers/pca8574/module.mk @@ -0,0 +1,6 @@ +# +# PCA8574 driver for RGB LED +# + +MODULE_COMMAND = pca8574 +SRCS = pca8574.cpp diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp new file mode 100644 index 000000000..ce882b636 --- /dev/null +++ b/src/drivers/pca8574/pca8574.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pca8574.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#define PCA8574_ONTIME 120 +#define PCA8574_OFFTIME 120 +#define PCA8574_DEVICE_PATH "/dev/pca8574" + +#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class PCA8574 : public device::I2C +{ +public: + PCA8574(int bus, int pca8574); + virtual ~PCA8574(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + work_s _work; + + uint8_t _values[8]; + float _brightness; + + enum IOX_MODE _mode; + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_values(); +}; + +/* for now, we only support one PCA8574 */ +namespace +{ +PCA8574 *g_pca8574; +} + +void pca8574_usage(); + +extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); + +PCA8574::PCA8574(int bus, int pca8574) : + I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _mode(IOX_MODE_OFF), + _values({}), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +PCA8574::~PCA8574() +{ +} + +int +PCA8574::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + + /* kick it in */ + _should_run = true; + _led_interval = 80; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); + + return OK; +} + +int +PCA8574::probe() +{ + + return send_led_enable(false); +} + +int +PCA8574::info() +{ + int ret = OK; + + return ret; +} + +int +PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): + { + /* set the specified color */ + unsigned prev = _values[cmd - IOX_SET_VALUE]; + _values[cmd - IOX_SET_VALUE] = arg; + if (_values[cmd - IOX_SET_VALUE] != prev) { + // XXX will be done with a change flag + send_led_values(); + } + return OK; + } + + case IOX_ENABLE: + send_led_enable(arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +PCA8574::led_trampoline(void *arg) +{ + PCA8574 *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +PCA8574::led() +{ + if (!_should_run) { + _running = false; + return; + } + + // switch (_mode) { + // case PCA8574_MODE_BLINK_SLOW: + // case PCA8574_MODE_BLINK_NORMAL: + // case PCA8574_MODE_BLINK_FAST: + // if (_counter >= 2) + // _counter = 0; + + // send_led_enable(_counter == 0); + + // break; + + // case PCA8574_MODE_BREATHE: + + // if (_counter >= 62) + // _counter = 0; + + // int n; + + // if (_counter < 32) { + // n = _counter; + + // } else { + // n = 62 - _counter; + // } + + // _brightness = n * n / (31.0f * 31.0f); + // send_led_rgb(); + // break; + + // case PCA8574_MODE_PATTERN: + + // /* don't run out of the pattern array and stop if the next frame is 0 */ + // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + // _counter = 0; + + // set_color(_pattern.color[_counter]); + // send_led_rgb(); + // _led_interval = _pattern.duration[_counter]; + // break; + + // default: + // break; + // } + + + // we count only seven states + _counter &= 0xF; + _counter++; + + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values[i] = 1; + } else { + _values[i] = 0; + } + } + + send_led_values(); + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); +} + +// /** +// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) +// */ +// void +// PCA8574::set_mode(pca8574_mode_t mode) +// { +// if (mode != _mode) { +// _mode = mode; + +// switch (mode) { +// // case PCA8574_MODE_OFF: +// // _should_run = false; +// // send_led_enable(false); +// // break; + +// // case PCA8574_MODE_ON: +// // _brightness = 1.0f; +// // send_led_rgb(); +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_BLINK_SLOW: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 2000; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_NORMAL: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 500; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_FAST: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 100; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BREATHE: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 25; +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_PATTERN: +// // _should_run = true; +// // _counter = 0; +// // _brightness = 1.0f; +// // send_led_enable(true); +// // break; + +// default: +// warnx("mode unknown"); +// break; +// } + +// /* if it should run now, start the workq */ +// if (_should_run && !_running) { +// _running = true; +// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); +// } + +// } +// } + +/** + * Sent ENABLE flag to LED driver + */ +int +PCA8574::send_led_enable(bool enable) +{ + uint8_t msg; + + if (enable) { + /* active low */ + msg = 0x00; + } else { + /* active low, so off */ + msg = 0xFF; + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); + + return ret; +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +PCA8574::send_led_values() +{ + uint8_t msg = 0; + + for (int i = 0; i < 8; i++) { + if (_values[i]) { + msg |= (1 << i); + } + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); +} + +// int +// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +// { +// uint8_t result[2]; +// int ret; + +// ret = transfer(nullptr, 0, &result[0], 2); + +// if (ret == OK) { +// on = result[0] & SETTING_ENABLE; +// powersave = !(result[0] & SETTING_NOT_POWERSAVE); +// /* XXX check, looks wrong */ +// r = (result[0] & 0x0f) << 4; +// g = (result[1] & 0xf0); +// b = (result[1] & 0x0f) << 4; +// } + +// return ret; +// } + +void +pca8574_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca8574_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int pca8574adr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + pca8574adr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca8574_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca8574_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca8574 != nullptr) + errx(1, "already started"); + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); + + if (g_pca8574 != nullptr && OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + } + + if (g_pca8574 == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_pca8574 == nullptr) { + g_pca8574 = new PCA8574(i2cdevice, pca8574adr); + + if (g_pca8574 == nullptr) + errx(1, "new failed"); + + if (OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_pca8574 == nullptr) { + warnx("not started"); + pca8574_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_VALUE, 255); + // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_pca8574->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } + + if (!strcmp(verb, "val")) { + if (argc < 4) { + errx(1, "Usage: pca8574 val "); + } + + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned val = strtol(argv[3], NULL, 0); + ret = ioctl(fd, (IOX_SET_VALUE+channel), val); + ret = ioctl(fd, IOX_ENABLE, 1); + close(fd); + exit(ret); + } + + pca8574_usage(); + exit(0); +} -- cgit v1.2.3 From 9a49636f6a6cfa631e8a4686e2cbb2f4e25770eb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 May 2014 13:15:17 +0200 Subject: position_estimator_inav: remove acceleration from state and INAV_W_XXX_ACC parameters, more NaN checks --- .../position_estimator_inav/inertial_filter.c | 15 +++-- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 68 ++++++++++------------ .../position_estimator_inav_params.c | 6 -- .../position_estimator_inav_params.h | 4 -- 5 files changed, 38 insertions(+), 57 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 2f1b3c014..a36a4688d 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -9,15 +9,18 @@ #include "inertial_filter.h" -void inertial_filter_predict(float dt, float x[3]) +void inertial_filter_predict(float dt, float x[2], float acc) { if (isfinite(dt)) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (!isfinite(acc)) { + acc = 0.0f; + } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; + x[1] += acc * dt; } } -void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[2], int i, float w) { if (isfinite(e) && isfinite(w) && isfinite(dt)) { float ewdt = e * w * dt; @@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; - - } else if (i == 1) { - x[2] += w * ewdt; } } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 761c17097..cdeb4cfc6 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -8,6 +8,6 @@ #include #include -void inertial_filter_predict(float dt, float x[3]); +void inertial_filter_predict(float dt, float x[3], float acc); void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index fdc3233e0..d7503e42d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -168,15 +168,15 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -195,14 +195,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - float x_est[3] = { 0.0f, 0.0f, 0.0f }; - float y_est[3] = { 0.0f, 0.0f, 0.0f }; - float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[2] = { 0.0f, 0.0f }; // pos, vel + float y_est[2] = { 0.0f, 0.0f }; // pos, vel + float z_est[2] = { 0.0f, 0.0f }; // pos, vel float eph = 1.0; float epv = 1.0; - float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); @@ -241,7 +241,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { @@ -341,8 +341,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_offset += sensor.baro_alt_meter; - baro_init_cnt++; + if (isfinite(sensor.baro_alt_meter)) { + baro_offset += sensor.baro_alt_meter; + baro_init_cnt++; + } } else { wait_baro = false; @@ -418,19 +420,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } - corr_acc[0] = accel_NED[0] - x_est[2]; - corr_acc[1] = accel_NED[1] - y_est[2]; - corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + acc[2] += CONSTANTS_ONE_G; } else { - memset(corr_acc, 0, sizeof(corr_acc)); + memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp; @@ -628,11 +628,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; - x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; z_est[0] = 0.0f; - y_est[2] = accel_NED[1]; local_pos.ref_lat = lat; local_pos.ref_lon = lon; @@ -655,10 +653,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; - x_est[2] = accel_NED[0]; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; - y_est[2] = accel_NED[1]; } /* calculate correction for position */ @@ -796,26 +792,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += att.R[j][i] * accel_bias_corr[j]; } - acc_bias[i] += c * params.w_acc_bias * dt; + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } } /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); + inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); corr_baro = 0; @@ -825,19 +821,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (can_estimate_xy) { /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); + inertial_filter_predict(dt, x_est, acc[0]); + inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ - inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); - inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); - if (use_flow) { inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); @@ -853,11 +846,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 2e4f26661..8aa08b6f2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -42,11 +42,9 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); -PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); @@ -62,11 +60,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_acc = param_find("INAV_W_Z_ACC"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_acc = param_find("INAV_W_XY_ACC"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); @@ -85,11 +81,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_acc, &(p->w_z_acc)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_acc, &(p->w_xy_acc)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index e2be079d3..08ec996a1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,11 +43,9 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; - float w_z_acc; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; - float w_xy_acc; float w_xy_flow; float w_gps_flow; float w_acc_bias; @@ -63,11 +61,9 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; - param_t w_z_acc; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; - param_t w_xy_acc; param_t w_xy_flow; param_t w_gps_flow; param_t w_acc_bias; -- cgit v1.2.3 From 17d8e2a1663b36504a432b120c3993e4912842b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 12:24:50 +0200 Subject: PCA8574 driver: Cleanup, ready for final testing and production --- src/drivers/drv_io_expander.h | 18 ++- src/drivers/pca8574/pca8574.cpp | 341 ++++++++++++++++------------------------ 2 files changed, 148 insertions(+), 211 deletions(-) diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h index 2705d6f9e..106354377 100644 --- a/src/drivers/drv_io_expander.h +++ b/src/drivers/drv_io_expander.h @@ -49,17 +49,23 @@ #define _IOXIOCBASE (0x2800) #define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) -/** enable the device */ -#define IOX_ENABLE _IOXIOC(1) +/** set a bitmask (non-blocking) */ +#define IOX_SET_MASK _IOXIOC(1) -/** set constant values */ -#define IOX_SET_VALUE _IOXIOC(2) +/** get a bitmask (blocking) */ +#define IOX_GET_MASK _IOXIOC(2) -/** set constant values */ +/** set device mode (non-blocking) */ #define IOX_SET_MODE _IOXIOC(3) +/** set constant values (non-blocking) */ +#define IOX_SET_VALUE _IOXIOC(4) + +/* ... to IOX_SET_VALUE + 8 */ + /* enum passed to RGBLED_SET_MODE ioctl()*/ enum IOX_MODE { IOX_MODE_OFF, - IOX_MODE_ON + IOX_MODE_ON, + IOX_MODE_TEST_OUT }; diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index ce882b636..cd1ffc82c 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Julian Oes - * Anton Babushkin + * Copyright (c) 2012-2014 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 @@ -36,8 +34,11 @@ /** * @file pca8574.cpp * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * Driver for an 8 I/O controller (PC8574) connected via I2C. * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -68,16 +69,7 @@ #define PCA8574_OFFTIME 120 #define PCA8574_DEVICE_PATH "/dev/pca8574" -#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ -#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ -#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ -#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ -#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ - -#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ -#define SETTING_ENABLE 0x02 /**< on */ - +#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) class PCA8574 : public device::I2C { @@ -94,20 +86,23 @@ public: private: work_s _work; - uint8_t _values[8]; - float _brightness; + uint8_t _values_out; + uint8_t _values_in; enum IOX_MODE _mode; bool _running; int _led_interval; bool _should_run; + bool _update_out; int _counter; static void led_trampoline(void *arg); void led(); - int send_led_enable(bool enable); + int send_led_enable(uint8_t arg); int send_led_values(); + + int get(uint8_t &vals); }; /* for now, we only support one PCA8574 */ @@ -122,12 +117,13 @@ extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _values_out(0), + _values_in(0), _mode(IOX_MODE_OFF), - _values({}), - _brightness(1.0f), _running(false), _led_interval(0), _should_run(false), + _update_out(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -147,10 +143,10 @@ PCA8574::init() return ret; } - /* switch off LED on start */ - send_led_enable(false); + // switch off LED on start (active low on Pixhawk) + send_led_enable(0xFF); - /* kick it in */ + // kick it in _should_run = true; _led_interval = 80; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); @@ -179,24 +175,69 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { - case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): - { - /* set the specified color */ - unsigned prev = _values[cmd - IOX_SET_VALUE]; - _values[cmd - IOX_SET_VALUE] = arg; - if (_values[cmd - IOX_SET_VALUE] != prev) { - // XXX will be done with a change flag - send_led_values(); + case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { + // set the specified on / off state + uint8_t position = (1 << (cmd - IOX_SET_VALUE)); + uint8_t prev = _values_out; + + if (arg) { + _values_out |= position; + + } else { + _values_out &= ~(position); + } + + if (_values_out != prev) { + send_led_values(); + } + + return OK; } - return OK; - } - case IOX_ENABLE: + case IOX_SET_MASK: send_led_enable(arg); return OK; + case IOX_GET_MASK: { + uint8_t val; + ret = get(val); + + if (ret == OK) { + return val; + + } else { + return -1; + } + } + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + _values_out = 0xFF; + break; + + case IOX_MODE_ON: + _values_out = 0; + break; + + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + send_led_values(); + } + + return OK; + default: - /* see if the parent class can make any use of it */ + // see if the parent class can make any use of it ret = CDev::ioctl(filp, cmd, arg); break; } @@ -224,204 +265,93 @@ PCA8574::led() return; } - // switch (_mode) { - // case PCA8574_MODE_BLINK_SLOW: - // case PCA8574_MODE_BLINK_NORMAL: - // case PCA8574_MODE_BLINK_FAST: - // if (_counter >= 2) - // _counter = 0; - - // send_led_enable(_counter == 0); + if (_mode == IOX_MODE_TEST_OUT) { - // break; + // we count only seven states + _counter &= 0xF; + _counter++; - // case PCA8574_MODE_BREATHE: + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values_out |= (1 << i); - // if (_counter >= 62) - // _counter = 0; - - // int n; - - // if (_counter < 32) { - // n = _counter; - - // } else { - // n = 62 - _counter; - // } - - // _brightness = n * n / (31.0f * 31.0f); - // send_led_rgb(); - // break; - - // case PCA8574_MODE_PATTERN: - - // /* don't run out of the pattern array and stop if the next frame is 0 */ - // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) - // _counter = 0; - - // set_color(_pattern.color[_counter]); - // send_led_rgb(); - // _led_interval = _pattern.duration[_counter]; - // break; + } else { + _values_out &= ~(1 << i); + } + } - // default: - // break; - // } + _update_out = true; + } + if (_update_out) { + uint8_t msg = _values_out; - // we count only seven states - _counter &= 0xF; - _counter++; + int ret = transfer(&msg, sizeof(msg), nullptr, 0); - for (int i = 0; i < 8; i++) { - if (i < _counter) { - _values[i] = 1; - } else { - _values[i] = 0; + if (!ret) { + _update_out = false; } } - send_led_values(); + // check if any activity remains, else stp + if (!_update_out) { + _running = false; + return; + } - /* re-queue ourselves to run again later */ + // re-queue ourselves to run again later work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } -// /** -// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) -// */ -// void -// PCA8574::set_mode(pca8574_mode_t mode) -// { -// if (mode != _mode) { -// _mode = mode; - -// switch (mode) { -// // case PCA8574_MODE_OFF: -// // _should_run = false; -// // send_led_enable(false); -// // break; - -// // case PCA8574_MODE_ON: -// // _brightness = 1.0f; -// // send_led_rgb(); -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_BLINK_SLOW: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 2000; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_NORMAL: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 500; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_FAST: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 100; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BREATHE: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 25; -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_PATTERN: -// // _should_run = true; -// // _counter = 0; -// // _brightness = 1.0f; -// // send_led_enable(true); -// // break; - -// default: -// warnx("mode unknown"); -// break; -// } - -// /* if it should run now, start the workq */ -// if (_should_run && !_running) { -// _running = true; -// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); -// } - -// } -// } - /** * Sent ENABLE flag to LED driver */ int -PCA8574::send_led_enable(bool enable) +PCA8574::send_led_enable(uint8_t arg) { - uint8_t msg; - - if (enable) { - /* active low */ - msg = 0x00; - } else { - /* active low, so off */ - msg = 0xFF; - } - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + int ret = transfer(&arg, sizeof(arg), nullptr, 0); return ret; } /** - * Send RGB PWM settings to LED driver according to current color and brightness + * Send 8 outputs */ int PCA8574::send_led_values() { - uint8_t msg = 0; + _update_out = true; + _should_run = true; - for (int i = 0; i < 8; i++) { - if (_values[i]) { - msg |= (1 << i); - } + // if not active, kick it + if (!_running) { + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + return 0; } -// int -// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) -// { -// uint8_t result[2]; -// int ret; +int +PCA8574::get(uint8_t &vals) +{ + uint8_t result; + int ret; -// ret = transfer(nullptr, 0, &result[0], 2); + ret = transfer(nullptr, 0, &result, 1); -// if (ret == OK) { -// on = result[0] & SETTING_ENABLE; -// powersave = !(result[0] & SETTING_NOT_POWERSAVE); -// /* XXX check, looks wrong */ -// r = (result[0] & 0x0f) << 4; -// g = (result[1] & 0xf0); -// b = (result[1] & 0x0f) << 4; -// } + if (ret == OK) { + _values_in = result; + vals = result; + } -// return ret; -// } + return ret; +} void pca8574_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -431,11 +361,11 @@ int pca8574_main(int argc, char *argv[]) { int i2cdevice = -1; - int pca8574adr = ADDR; /* 7bit */ + int pca8574adr = ADDR; // 7bit int ch; - /* jump over start/off/etc and look at options first */ + // jump over start/off/etc and look at options first while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': @@ -452,10 +382,10 @@ pca8574_main(int argc, char *argv[]) } } - if (optind >= argc) { - pca8574_usage(); - exit(1); - } + if (optind >= argc) { + pca8574_usage(); + exit(1); + } const char *verb = argv[optind]; @@ -463,8 +393,9 @@ pca8574_main(int argc, char *argv[]) int ret; if (!strcmp(verb, "start")) { - if (g_pca8574 != nullptr) + if (g_pca8574 != nullptr) { errx(1, "already started"); + } if (i2cdevice == -1) { // try the external bus first @@ -481,6 +412,7 @@ pca8574_main(int argc, char *argv[]) if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { errx(1, "init failed"); } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -488,8 +420,9 @@ pca8574_main(int argc, char *argv[]) if (g_pca8574 == nullptr) { g_pca8574 = new PCA8574(i2cdevice, pca8574adr); - if (g_pca8574 == nullptr) + if (g_pca8574 == nullptr) { errx(1, "new failed"); + } if (OK != g_pca8574->init()) { delete g_pca8574; @@ -501,7 +434,7 @@ pca8574_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + // need the driver past this point if (g_pca8574 == nullptr) { warnx("not started"); pca8574_usage(); @@ -515,8 +448,7 @@ pca8574_main(int argc, char *argv[]) errx(1, "Unable to open " PCA8574_DEVICE_PATH); } - ret = ioctl(fd, IOX_SET_VALUE, 255); - // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); close(fd); exit(ret); @@ -547,7 +479,7 @@ pca8574_main(int argc, char *argv[]) if (!strcmp(verb, "val")) { if (argc < 4) { - errx(1, "Usage: pca8574 val "); + errx(1, "Usage: pca8574 val <0 or 1>"); } fd = open(PCA8574_DEVICE_PATH, 0); @@ -558,8 +490,7 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE+channel), val); - ret = ioctl(fd, IOX_ENABLE, 1); + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); close(fd); exit(ret); } -- cgit v1.2.3 From 0c2e70d30f3e1d927a22fdd066a4a14ecc698de1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:29 +0200 Subject: Enable the driver as default for boards having it --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c413e087..87ec4293e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -93,14 +93,18 @@ then # if rgbled start then - echo "[init] Using external RGB Led" + echo "[init] RGB Led" else if blinkm start then - echo "[init] Using blinkm" + echo "[init] BlinkM" blinkm systemstate fi fi + + if pca8574 start + then + fi # # Set default values -- cgit v1.2.3 From eb02e74d30a215ff415983913bf569a23e916a2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:47 +0200 Subject: PCA cleanup: Full interfaces ready for AP use --- src/drivers/pca8574/pca8574.cpp | 108 ++++++++++++++++++++++++++++++---------- 1 file changed, 81 insertions(+), 27 deletions(-) diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index cd1ffc82c..904ce18e8 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -82,12 +82,15 @@ public: virtual int probe(); virtual int info(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } private: work_s _work; uint8_t _values_out; uint8_t _values_in; + uint8_t _blinking; + uint8_t _blink_phase; enum IOX_MODE _mode; bool _running; @@ -119,9 +122,11 @@ PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), _values_out(0), _values_in(0), + _blinking(0), + _blink_phase(0), _mode(IOX_MODE_OFF), _running(false), - _led_interval(0), + _led_interval(80), _should_run(false), _update_out(false), _counter(0) @@ -143,22 +148,14 @@ PCA8574::init() return ret; } - // switch off LED on start (active low on Pixhawk) - send_led_enable(0xFF); - - // kick it in - _should_run = true; - _led_interval = 80; - work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); - return OK; } int PCA8574::probe() { - - return send_led_enable(false); + uint8_t val; + return get(val); } int @@ -188,6 +185,9 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) } if (_values_out != prev) { + if (_values_out) { + _mode = IOX_MODE_ON; + } send_led_values(); } @@ -260,11 +260,6 @@ PCA8574::led_trampoline(void *arg) void PCA8574::led() { - if (!_should_run) { - _running = false; - return; - } - if (_mode == IOX_MODE_TEST_OUT) { // we count only seven states @@ -281,10 +276,37 @@ PCA8574::led() } _update_out = true; + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _update_out = true; + _should_run = false; + } else { + + // Any of the normal modes + if (_blinking > 0) { + /* we need to be running to blink */ + _should_run = true; + } else { + _should_run = false; + } } if (_update_out) { - uint8_t msg = _values_out; + uint8_t msg; + + if (_blinking) { + msg = (_values_out & _blinking & _blink_phase); + + // wipe out all positions that are marked as blinking + msg &= ~(_blinking); + + // fill blink positions + msg |= ((_blink_phase) ? _blinking : 0); + + _blink_phase = !_blink_phase; + } else { + msg = _values_out; + } int ret = transfer(&msg, sizeof(msg), nullptr, 0); @@ -294,12 +316,13 @@ PCA8574::led() } // check if any activity remains, else stp - if (!_update_out) { + if (!_should_run) { _running = false; return; } // re-queue ourselves to run again later + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } @@ -322,10 +345,10 @@ int PCA8574::send_led_values() { _update_out = true; - _should_run = true; // if not active, kick it if (!_running) { + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } @@ -436,8 +459,7 @@ pca8574_main(int argc, char *argv[]) // need the driver past this point if (g_pca8574 == nullptr) { - warnx("not started"); - pca8574_usage(); + warnx("not started, run pca8574 start"); exit(1); } @@ -459,10 +481,10 @@ pca8574_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + if (!strcmp(verb, "off")) { fd = open(PCA8574_DEVICE_PATH, 0); - if (fd == -1) { + if (fd < 0) { errx(1, "Unable to open " PCA8574_DEVICE_PATH); } @@ -472,9 +494,36 @@ pca8574_main(int argc, char *argv[]) } if (!strcmp(verb, "stop")) { - delete g_pca8574; - g_pca8574 = nullptr; - exit(0); + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca8574->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca8574->is_running()) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } else { + warnx("stop failed."); + exit(1); + } } if (!strcmp(verb, "val")) { @@ -490,7 +539,12 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + + if (channel < 8) { + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { + ret = -1; + } close(fd); exit(ret); } -- cgit v1.2.3 From 46301f753d212b55e151a394bc9d4b3b787f35ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 18:28:37 +0200 Subject: Minor fixes to MAVLink --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef..28cdf65a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), -- cgit v1.2.3 From 718206bd6d4103d8d2b1ad6a111770f65622029a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 14:37:23 +0200 Subject: ubx driver: fix unit of speed and position accuracy estimate --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..7502809bd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -447,8 +447,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; -- cgit v1.2.3