forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'upstream/master' into mtecs
This commit is contained in:
Thomas Gubler
commit
c94e358380
|
|
@ -93,15 +93,19 @@ 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
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*/
|
||||
|
||||
#define _IOXIOCBASE (0x2800)
|
||||
#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n))
|
||||
|
||||
/** set a bitmask (non-blocking) */
|
||||
#define IOX_SET_MASK _IOXIOC(1)
|
||||
|
||||
/** get a bitmask (blocking) */
|
||||
#define IOX_GET_MASK _IOXIOC(2)
|
||||
|
||||
/** 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_TEST_OUT
|
||||
};
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
#
|
||||
# PCA8574 driver for RGB LED
|
||||
#
|
||||
|
||||
MODULE_COMMAND = pca8574
|
||||
SRCS = pca8574.cpp
|
||||
|
|
@ -0,0 +1,554 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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 an 8 I/O controller (PC8574) connected via I2C.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include <drivers/drv_io_expander.h>
|
||||
|
||||
#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)
|
||||
|
||||
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);
|
||||
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;
|
||||
int _led_interval;
|
||||
bool _should_run;
|
||||
bool _update_out;
|
||||
int _counter;
|
||||
|
||||
static void led_trampoline(void *arg);
|
||||
void led();
|
||||
|
||||
int send_led_enable(uint8_t arg);
|
||||
int send_led_values();
|
||||
|
||||
int get(uint8_t &vals);
|
||||
};
|
||||
|
||||
/* 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),
|
||||
_values_out(0),
|
||||
_values_in(0),
|
||||
_blinking(0),
|
||||
_blink_phase(0),
|
||||
_mode(IOX_MODE_OFF),
|
||||
_running(false),
|
||||
_led_interval(80),
|
||||
_should_run(false),
|
||||
_update_out(false),
|
||||
_counter(0)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
PCA8574::~PCA8574()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::init()
|
||||
{
|
||||
int ret;
|
||||
ret = I2C::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::probe()
|
||||
{
|
||||
uint8_t val;
|
||||
return get(val);
|
||||
}
|
||||
|
||||
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 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) {
|
||||
if (_values_out) {
|
||||
_mode = IOX_MODE_ON;
|
||||
}
|
||||
send_led_values();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
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
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
PCA8574::led_trampoline(void *arg)
|
||||
{
|
||||
PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg);
|
||||
|
||||
rgbl->led();
|
||||
}
|
||||
|
||||
/**
|
||||
* Main loop function
|
||||
*/
|
||||
void
|
||||
PCA8574::led()
|
||||
{
|
||||
if (_mode == IOX_MODE_TEST_OUT) {
|
||||
|
||||
// we count only seven states
|
||||
_counter &= 0xF;
|
||||
_counter++;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (i < _counter) {
|
||||
_values_out |= (1 << i);
|
||||
|
||||
} else {
|
||||
_values_out &= ~(1 << i);
|
||||
}
|
||||
}
|
||||
|
||||
_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;
|
||||
|
||||
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);
|
||||
|
||||
if (!ret) {
|
||||
_update_out = false;
|
||||
}
|
||||
}
|
||||
|
||||
// check if any activity remains, else stp
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sent ENABLE flag to LED driver
|
||||
*/
|
||||
int
|
||||
PCA8574::send_led_enable(uint8_t arg)
|
||||
{
|
||||
|
||||
int ret = transfer(&arg, sizeof(arg), nullptr, 0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Send 8 outputs
|
||||
*/
|
||||
int
|
||||
PCA8574::send_led_values()
|
||||
{
|
||||
_update_out = true;
|
||||
|
||||
// if not active, kick it
|
||||
if (!_running) {
|
||||
_running = true;
|
||||
work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
PCA8574::get(uint8_t &vals)
|
||||
{
|
||||
uint8_t result;
|
||||
int ret;
|
||||
|
||||
ret = transfer(nullptr, 0, &result, 1);
|
||||
|
||||
if (ret == OK) {
|
||||
_values_in = result;
|
||||
vals = result;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
pca8574_usage()
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
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, run pca8574 start");
|
||||
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_MODE, (unsigned long)IOX_MODE_TEST_OUT);
|
||||
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "info")) {
|
||||
g_pca8574->info();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "off")) {
|
||||
fd = open(PCA8574_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
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")) {
|
||||
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")) {
|
||||
if (argc < 4) {
|
||||
errx(1, "Usage: pca8574 val <channel> <0 or 1>");
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
if (channel < 8) {
|
||||
ret = ioctl(fd, (IOX_SET_VALUE + channel), val);
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
close(fd);
|
||||
exit(ret);
|
||||
}
|
||||
|
||||
pca8574_usage();
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,6 @@
|
|||
#include <stdbool.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
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];
|
||||
}
|
||||
|
||||
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));
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue