dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/master' into mtecs

This commit is contained in:
Thomas Gubler 2014-05-30 19:36:19 +02:00
parent 33356ed503 90d8f7726d
commit c94e358380
13 changed files with 684 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -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,

1
makefiles/config_px4fmu-v2_test.mk
View File

@ -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

71
src/drivers/drv_io_expander.h Normal file
View File

@ -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
};

4
src/drivers/gps/ubx.cpp
View File

@ -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;

6
src/drivers/pca8574/module.mk Normal file
View File

@ -0,0 +1,6 @@
#
# PCA8574 driver for RGB LED
#
MODULE_COMMAND = pca8574
SRCS = pca8574.cpp

554
src/drivers/pca8574/pca8574.cpp Normal file
View File

@ -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);
}

7
src/modules/mavlink/mavlink_main.cpp
View File

@ -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),

15
src/modules/position_estimator_inav/inertial_filter.c
View File

@ -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;
}
}
}

2
src/modules/position_estimator_inav/inertial_filter.h
View File

@ -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);

68
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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));

6
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -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));

4
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -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;