From 09b0de21f3bbd56768aa5a5b0f43930ff5f995ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:17:24 +0100 Subject: [PATCH 001/134] Deleted backside config to cut down build times --- makefiles/config_px4fmu-v1_backside.mk | 161 ------------------------- 1 file changed, 161 deletions(-) delete mode 100644 makefiles/config_px4fmu-v1_backside.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index 5ecf93a3a1..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,161 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/mkblctrl -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -#MODULES += modules/attitude_estimator_ekf -MODULES += modules/att_pos_estimator_ekf -#MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator -MODULES += modules/attitude_estimator_so3 - -# -# Vehicle Control -# -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control -#MODULES += modules/multirotor_att_control -#MODULES += modules/multirotor_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) From 26d26a9b379c03214fed4b47fb8a4f6896177fb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:17:47 +0100 Subject: [PATCH 002/134] Removed fp suffix warning --- makefiles/toolchain_gnu-arm-eabi.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 9fd2dd516d..bb729e1032 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs # C++-specific warnings # From ffe5c88d98b18390e3d44c8dd0751b6d062631be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:18:21 +0100 Subject: [PATCH 003/134] Added new estimator framework, actual estimation code not yet present --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../fw_att_pos_estimator_main.cpp | 456 ++++++++++++++++++ .../fw_att_pos_estimator_params.c | 52 ++ src/modules/fw_att_pos_estimator/module.mk | 41 ++ 5 files changed, 551 insertions(+), 2 deletions(-) create mode 100644 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp create mode 100644 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c create mode 100644 src/modules/fw_att_pos_estimator/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 25c22f1fdf..5798e9c230 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -73,7 +73,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf +MODULES += modules/fw_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 68673c0552..7614387a5f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,7 +73,7 @@ MODULES += modules/mavlink_onboard # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf +MODULES += modules/fw_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp new file mode 100644 index 0000000000..459c5c2f7d --- /dev/null +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -0,0 +1,456 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 fw_att_pos_estimator_main.cpp + * Implementation of the attitude and position estimator. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * estimator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); + +class FixedwingEstimator +{ +public: + /** + * Constructor + */ + FixedwingEstimator(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingEstimator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _estimator_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _gyro_sub; /**< gyro sensor subscription */ + int _accel_sub; /**< accel sensor subscription */ + int _mag_sub; /**< mag sensor subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + + orb_advert_t _att_pub; /**< position setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct gyro_report _gyro; + struct accel_report _accel; + struct mag_report _mag; + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + struct { + float throttle_cruise; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t throttle_cruise; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace estimator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingEstimator *g_estimator; +} + +FixedwingEstimator::FixedwingEstimator() : + + _task_should_exit(false), + _estimator_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _att_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), +/* states */ + _mission_items_maxcount(20), + _mission_valid(false), + _loiter_hold(false) +{ + + _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingEstimator::~FixedwingEstimator() +{ + if (_estimator_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_estimator_task); + break; + } + } while (_estimator_task != -1); + } + + estimator::g_estimator = nullptr; +} + +int +FixedwingEstimator::parameters_update() +{ + + //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + return OK; +} + +void +FixedwingEstimator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +{ + estimator::g_estimator->task_main(); +} + +void +FixedwingEstimator::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + parameters_update(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _gyro_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run estimator if gyro updated */ + if (fds[1].revents & POLLIN) { + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + + bool updated; + + orb_check(_accel_sub, &updated); + if (updated) + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + + orb_check(_mag_sub, &updated); + if (updated) + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + + orb_check(_airspeed_sub, &updated); + if (updated) + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _estimator_task = -1; + _exit(0); +} + +int +FixedwingEstimator::start() +{ + ASSERT(_estimator_task == -1); + + /* start the task */ + _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingEstimator::task_main_trampoline, + nullptr); + + if (_estimator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_pos_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (estimator::g_estimator != nullptr) + errx(1, "already running"); + + estimator::g_estimator = new FixedwingEstimator; + + if (estimator::g_estimator == nullptr) + errx(1, "alloc failed"); + + if (OK != estimator::g_estimator->start()) { + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (estimator::g_estimator == nullptr) + errx(1, "not running"); + + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (estimator::g_estimator) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c new file mode 100644 index 0000000000..64e593a15f --- /dev/null +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 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 fw_att_pos_estimator_params.c + * + * Parameters defined by the attitude and position estimator task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Estimator parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); + diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk new file mode 100644 index 0000000000..62bf60b60a --- /dev/null +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013, 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. +# +############################################################################ + +# +# Main Attitude and Position Estimator for Fixed Wing Aircraft +# + +MODULE_COMMAND = navigator + +SRCS = fw_att_pos_estimator_main.cpp \ + fw_att_pos_estimator_params.c From 5008f79a96db66f88153c264b81db2a720b560c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 16:48:15 +0100 Subject: [PATCH 004/134] Interfaced Pauls estimator, compiles, untested --- .../fw_att_pos_estimator_main.cpp | 187 +++++++++++++++--- src/modules/fw_att_pos_estimator/module.mk | 5 +- 2 files changed, 158 insertions(+), 34 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 459c5c2f7d..0f9c8ca821 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,8 @@ #include #include +#include "../../../InertialNav/code/estimator.h" + /** * estimator app start / stop handling function * @@ -78,6 +81,14 @@ */ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +__EXPORT uint32_t millis(); + +static uint64_t last_run = 0; + +uint32_t millis() { + return last_run / 1e3; +} + class FixedwingEstimator { public: @@ -103,18 +114,19 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _estimator_task; /**< task handle for sensor task */ - int _global_pos_sub; int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ + int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; - orb_advert_t _att_pub; /**< position setpoint */ + orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _global_pos_pub; /**< global position */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -125,22 +137,19 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_gps_position_s _gps; /**< GPS position */ perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ - - /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; + bool _initialized; struct { - float throttle_cruise; + float throttle_cruise; + uint32_t vel_delay_ms; + uint32_t pos_delay_ms; + uint32_t height_delay_ms; + uint32_t mag_delay_ms; + uint32_t tas_delay_ms; } _parameters; /**< local copies of interesting parameters */ struct { @@ -194,24 +203,23 @@ FixedwingEstimator::FixedwingEstimator() : _estimator_task(-1), /* subscriptions */ - _global_pos_sub(-1), _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), _airspeed_sub(-1), + _gps_sub(-1), _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), /* publications */ _att_pub(-1), + _global_pos_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), /* states */ - _mission_items_maxcount(20), - _mission_valid(false), - _loiter_hold(false) + _initialized(false) { _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); @@ -249,8 +257,15 @@ int FixedwingEstimator::parameters_update() { + // XXX NEED TO GET HANDLES FIRST! NEEDS PARAM INIT //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + _parameters.vel_delay_ms = 230; + _parameters.pos_delay_ms = 210; + _parameters.height_delay_ms = 350; + _parameters.mag_delay_ms = 30; + _parameters.tas_delay_ms = 210; + return OK; } @@ -295,8 +310,10 @@ FixedwingEstimator::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); - /* rate limit position updates to 50 Hz */ - orb_set_interval(_global_pos_sub, 20); + /* rate limit gyro updates to 50 Hz */ + + /* XXX remove this!, BUT increase the data buffer size! */ + orb_set_interval(_gyro_sub, 17); parameters_update(); @@ -342,31 +359,127 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + + + /* load local copies */ + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + + float IMUmsec = _gyro.timestamp / 1e6; + + float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; + last_run = _gyro.timestamp / 1e6 ; /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; - /* load local copies */ - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + dt = deltaT; - bool updated; + if (_initialized) { - orb_check(_accel_sub, &updated); - if (updated) - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + /* predict states and covariances */ - orb_check(_mag_sub, &updated); - if (updated) + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(); + + /* evaluate if on ground */ + OnGroundCheck(); + + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } + } + + bool gps_updated; + orb_check(_gps_sub, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + + if (_gps.fix_type > 2 && !_initialized) { + + /* initialize */ + InitialiseFilter(); + + } else if (_gps.fix_type > 2) { + /* fuse GPS updates */ + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + hgtMea = -posNED[2]; + + /* set flags for further processing */ + fuseVelData = true; + fusePosData = true; + fuseHgtData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + + } else { + + /* do not fuse anything, we got no position / vel update */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + } + } + + bool mag_updated; + + orb_check(_mag_sub, &mag_updated); + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); - orb_check(_airspeed_sub, &updated); - if (updated) + if (_initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); + FuseMagnetometer(); + } + + } else { + fuseMagData = false; + } + + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } + + } else { + fuseVtasData = false; + } + /* lazily publish the attitude only once available */ if (_att_pub > 0) { /* publish the attitude setpoint */ @@ -377,6 +490,16 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + /* lazily publish the position only once available */ + if (_global_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + } perf_end(_loop_perf); diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk index 62bf60b60a..c1384be7e9 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -35,7 +35,8 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = navigator +MODULE_COMMAND = fw_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ - fw_att_pos_estimator_params.c + fw_att_pos_estimator_params.c \ + ../../../../InertialNav/code/estimator.cpp From 75e6cf45e9fe46d65d6fdf1e48b75b2b329f0423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 17:04:24 +0100 Subject: [PATCH 005/134] Use a really large amount of stack to avoid running out of it --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0f9c8ca821..90d02843d1 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -520,7 +520,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 12048, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); From 3d68c37d3cb6eb0d177df5e90e6365cf27a5b483 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 16:25:53 +0100 Subject: [PATCH 006/134] Now actually loading data into the filter when running it --- .../fw_att_pos_estimator_main.cpp | 108 +++++++++++++++--- 1 file changed, 94 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 90d02843d1..b359348ed8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -85,7 +85,8 @@ __EXPORT uint32_t millis(); static uint64_t last_run = 0; -uint32_t millis() { +uint32_t millis() +{ return last_run / 1e3; } @@ -139,6 +140,10 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct gyro_scale _gyro_offsets; + struct accel_scale _accel_offsets; + struct mag_scale _mag_offsets; + perf_counter_t _loop_perf; /**< loop performance counter */ bool _initialized; @@ -226,6 +231,31 @@ FixedwingEstimator::FixedwingEstimator() : /* fetch initial parameter values */ parameters_update(); + + /* get offsets */ + + int fd, res; + + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); + close(fd); + } + + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); + close(fd); + } + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); + close(fd); + } } FixedwingEstimator::~FixedwingEstimator() @@ -317,6 +347,9 @@ FixedwingEstimator::task_main() parameters_update(); + Vector3f lastAngRate; + Vector3f lastAccel; + /* wakeup source(s) */ struct pollfd fds[2]; @@ -359,13 +392,13 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - + /* load local copies */ orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - float IMUmsec = _gyro.timestamp / 1e6; + float IMUmsec = _gyro.timestamp / 1e3; float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; last_run = _gyro.timestamp / 1e6 ; @@ -378,6 +411,22 @@ FixedwingEstimator::task_main() if (_initialized) { + /* fill in last data set */ + dtIMU = dt; + + angRate.x = _gyro.x; + angRate.y = _gyro.y; + angRate.z = _gyro.z; + + accel.x = _accel.x; + accel.y = _accel.y; + accel.z = _accel.z; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + /* predict states and covariances */ /* run the strapdown INS every sensor update */ @@ -407,17 +456,29 @@ FixedwingEstimator::task_main() bool gps_updated; orb_check(_gps_sub, &gps_updated); + if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); - if (_gps.fix_type > 2 && !_initialized) { - - /* initialize */ - InitialiseFilter(); - - } else if (_gps.fix_type > 2) { + if (_gps.fix_type > 2) { /* fuse GPS updates */ + //_gps.timestamp / 1e3; + GPSstatus = _gps.fix_type; + gpsCourse = _gps.cog_rad; + gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); + gpsVelD = _gps.vel_d_m_s; + gpsLat = math::radians(_gps.lat / (double)1e7); + gpsLon = math::radians(_gps.lon / (double)1e7); + gpsHgt = _gps.alt / 1e3f; + + newDataGps = true; + + if (!_initialized) { + InitialiseFilter(); + continue; + } + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); @@ -445,16 +506,32 @@ FixedwingEstimator::task_main() fuseVelData = false; fusePosData = false; fuseHgtData = false; + + newDataGps = true; } + + } else { + newDataGps = false; } bool mag_updated; orb_check(_mag_sub, &mag_updated); + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); if (_initialized) { + + magData.x = _mag.x; + magBias.x = -_mag_offsets.x_offset; + + magData.y = _mag.y; + magBias.y = -_mag_offsets.y_offset; + + magData.z = _mag.z; + magBias.z = -_mag_offsets.z_offset; + fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); FuseMagnetometer(); @@ -466,11 +543,14 @@ FixedwingEstimator::task_main() bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + VtasMeas = _airspeed.true_airspeed_m_s; + fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); @@ -518,11 +598,11 @@ FixedwingEstimator::start() /* start the task */ _estimator_task = task_spawn_cmd("fw_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12048, - (main_t)&FixedwingEstimator::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12048, + (main_t)&FixedwingEstimator::task_main_trampoline, + nullptr); if (_estimator_task < 0) { warn("task start failed"); From f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 16:59:11 +0100 Subject: [PATCH 007/134] Set new filter as default in HIL and fixed wing, fixes in estimator, WIP --- .../init.d/1000_rc_fw_easystar.hil | 2 +- .../init.d/1004_rc_fw_Rascal110.hil | 2 +- ROMFS/px4fmu_common/init.d/rc.fixedwing | 2 +- .../fw_att_pos_estimator_main.cpp | 76 +++++++++++-------- 4 files changed, 47 insertions(+), 35 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 40a13b5d17..181256dc71 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -74,7 +74,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Load mixer and start controllers (depends on px4io) diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 344d784224..858b6572ef 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -74,7 +74,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Load mixer and start controllers (depends on px4io) diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing index f028510064..c3f24cfc4e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -21,7 +21,7 @@ gps start # # Start the attitude and position estimator # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Start attitude controller diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index b359348ed8..adf679d7e0 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -334,6 +334,7 @@ FixedwingEstimator::task_main() _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -349,6 +350,12 @@ FixedwingEstimator::task_main() Vector3f lastAngRate; Vector3f lastAccel; + /* set initial filter state */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; /* wakeup source(s) */ struct pollfd fds[2]; @@ -359,6 +366,8 @@ FixedwingEstimator::task_main() fds[1].fd = _gyro_sub; fds[1].events = POLLIN; + hrt_abstime start_time = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -376,9 +385,6 @@ FixedwingEstimator::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -392,7 +398,8 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); /* load local copies */ orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -407,12 +414,10 @@ FixedwingEstimator::task_main() if (deltaT > 1.0f) deltaT = 0.01f; - dt = deltaT; - if (_initialized) { /* fill in last data set */ - dtIMU = dt; + dtIMU = deltaT; angRate.x = _gyro.x; angRate.y = _gyro.y; @@ -456,7 +461,6 @@ FixedwingEstimator::task_main() bool gps_updated; orb_check(_gps_sub, &gps_updated); - if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); @@ -474,8 +478,10 @@ FixedwingEstimator::task_main() newDataGps = true; - if (!_initialized) { + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); + _initialized = true; + warnx("init done."); continue; } @@ -487,7 +493,7 @@ FixedwingEstimator::task_main() posNE[1] = posNED[1]; hgtMea = -posNED[2]; - /* set flags for further processing */ + // set flags for further processing fuseVelData = true; fusePosData = true; fuseHgtData = true; @@ -507,7 +513,7 @@ FixedwingEstimator::task_main() fusePosData = false; fuseHgtData = false; - newDataGps = true; + newDataGps = false; } } else { @@ -517,8 +523,7 @@ FixedwingEstimator::task_main() bool mag_updated; orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { + if (mag_updated && _initialized) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); if (_initialized) { @@ -543,41 +548,48 @@ FixedwingEstimator::task_main() bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { + if (airspeed_updated && _initialized) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { VtasMeas = _airspeed.true_airspeed_m_s; fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); + } else { + fuseVtasData = false; } } else { fuseVtasData = false; } - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + if (_initialized) { - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + math::EulerAngles euler(eulerEst[0], eulerEst[1], eulerEst[2]); + //warnx("est: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); - /* lazily publish the position only once available */ - if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + // /* lazily publish the attitude only once available */ + // if (_att_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } else { + // /* advertise and publish */ + // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + // } + + // /* lazily publish the position only once available */ + // if (_global_pos_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + // } else { + // /* advertise and publish */ + // _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } } } @@ -599,7 +611,7 @@ FixedwingEstimator::start() /* start the task */ _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX - 40, 12048, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); From 17ac30f1b195b5095100abb5d595c781060f218d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:08:24 +0100 Subject: [PATCH 008/134] More fixes, but things are pretty NaN still --- .../fw_att_pos_estimator_main.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index adf679d7e0..b238422b63 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -407,8 +407,8 @@ FixedwingEstimator::task_main() float IMUmsec = _gyro.timestamp / 1e3; - float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; - last_run = _gyro.timestamp / 1e6 ; + float deltaT = (_gyro.timestamp - last_run) / 1e6f; + last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (deltaT > 1.0f) @@ -659,7 +659,12 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (estimator::g_estimator) { - errx(0, "running"); + warnx("running"); + warnx("attitude: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); + warnx("states [1-3]: %8.4f, %8.4f, %8.4f", states[0], states[1], states[2]); + warnx("states [4-6]: %8.4f, %8.4f, %8.4f", states[3], states[4], states[5]); + warnx("states [7-9]: %8.4f, %8.4f, %8.4f", states[6], states[7], states[8]); + exit(0); } else { errx(1, "not running"); From cbac002fb3997c2d3a4cf47ab0fe81c8020baaff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 19:18:19 +0100 Subject: [PATCH 009/134] Use the already compensated mag vector and initialize offsets to zero --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index b238422b63..49622dda38 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -528,14 +528,15 @@ FixedwingEstimator::task_main() if (_initialized) { + // XXX we compensate the offsets upfront - should be close to zero. magData.x = _mag.x; - magBias.x = -_mag_offsets.x_offset; + magBias.x = 0.0f; // _mag_offsets.x_offset magData.y = _mag.y; - magBias.y = -_mag_offsets.y_offset; + magBias.y = 0.0f; // _mag_offsets.y_offset magData.z = _mag.z; - magBias.z = -_mag_offsets.z_offset; + magBias.z = 0.0f; // _mag_offsets.y_offset fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); From 2bf4e7912436e3fde31567a9419a5d861d58b6c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 20:08:39 +0100 Subject: [PATCH 010/134] Fix up init --- .../fw_att_pos_estimator_main.cpp | 122 ++++++++++-------- 1 file changed, 69 insertions(+), 53 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 49622dda38..1ed526e83f 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -414,24 +414,26 @@ FixedwingEstimator::task_main() if (deltaT > 1.0f) deltaT = 0.01f; + + // Always store data, independent of init status + /* fill in last data set */ + dtIMU = deltaT; + + angRate.x = _gyro.x; + angRate.y = _gyro.y; + angRate.z = _gyro.z; + + accel.x = _accel.x; + accel.y = _accel.y; + accel.z = _accel.z; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + if (_initialized) { - /* fill in last data set */ - dtIMU = deltaT; - - angRate.x = _gyro.x; - angRate.y = _gyro.y; - angRate.z = _gyro.z; - - accel.x = _accel.x; - accel.y = _accel.y; - accel.z = _accel.z; - - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; - /* predict states and covariances */ /* run the strapdown INS every sensor update */ @@ -485,26 +487,29 @@ FixedwingEstimator::task_main() continue; } - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + if (_initialized) { - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + hgtMea = -posNED[2]; - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // set flags for further processing + fuseVelData = true; + fusePosData = true; + fuseHgtData = true; - /* run the actual fusion */ - FuseVelposNED(); + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + } } else { @@ -521,23 +526,22 @@ FixedwingEstimator::task_main() } bool mag_updated; - orb_check(_mag_sub, &mag_updated); - if (mag_updated && _initialized) { + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + // XXX we compensate the offsets upfront - should be close to zero. + magData.x = _mag.x; + magBias.x = 0.0f; // _mag_offsets.x_offset + + magData.y = _mag.y; + magBias.y = 0.0f; // _mag_offsets.y_offset + + magData.z = _mag.z; + magBias.z = 0.0f; // _mag_offsets.y_offset + if (_initialized) { - // XXX we compensate the offsets upfront - should be close to zero. - magData.x = _mag.x; - magBias.x = 0.0f; // _mag_offsets.x_offset - - magData.y = _mag.y; - magBias.y = 0.0f; // _mag_offsets.y_offset - - magData.z = _mag.z; - magBias.z = 0.0f; // _mag_offsets.y_offset - fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); FuseMagnetometer(); @@ -556,9 +560,12 @@ FixedwingEstimator::task_main() VtasMeas = _airspeed.true_airspeed_m_s; - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); + if (_initialized) { + + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } } else { fuseVtasData = false; } @@ -569,8 +576,11 @@ FixedwingEstimator::task_main() if (_initialized) { - math::EulerAngles euler(eulerEst[0], eulerEst[1], eulerEst[2]); - //warnx("est: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); + math::Quaternion q(states[0], states[1], states[2], states[3]); + + _att.q = q; + _att.q_valid = true; + _att.R_valid = false; // /* lazily publish the attitude only once available */ // if (_att_pub > 0) { @@ -661,10 +671,16 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (estimator::g_estimator) { warnx("running"); - warnx("attitude: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); - warnx("states [1-3]: %8.4f, %8.4f, %8.4f", states[0], states[1], states[2]); - warnx("states [4-6]: %8.4f, %8.4f, %8.4f", states[3], states[4], states[5]); - warnx("states [7-9]: %8.4f, %8.4f, %8.4f", states[6], states[7], states[8]); + + math::Quaternion q(states[0], states[1], states[2], states[3]); + math::EulerAngles euler(q); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", + math::degrees(euler.getPhi()), math::degrees(euler.getTheta()), math::degrees(euler.getPsi())); + + printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", states[0], states[1], states[2]); + printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", states[3], states[4], states[5]); + printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", states[6], states[7], states[8]); exit(0); } else { From 421727274251e5fc81ca5ae503a180b7326a480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 20:17:53 +0100 Subject: [PATCH 011/134] Filter returns some results. Needs careful range and unit checking, but on track --- .../fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1ed526e83f..59fb3dc31a 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -578,9 +578,9 @@ FixedwingEstimator::task_main() math::Quaternion q(states[0], states[1], states[2], states[3]); - _att.q = q; - _att.q_valid = true; - _att.R_valid = false; + // _att.q[0] = q.r; + // _att.q_valid = true; + // _att.R_valid = false; // /* lazily publish the attitude only once available */ // if (_att_pub > 0) { @@ -623,7 +623,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 12048, + 6000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); @@ -676,11 +676,11 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) math::EulerAngles euler(q); printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", - math::degrees(euler.getPhi()), math::degrees(euler.getTheta()), math::degrees(euler.getPsi())); + (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", states[0], states[1], states[2]); - printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", states[3], states[4], states[5]); - printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", states[6], states[7], states[8]); + printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2]); + printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", (double)states[3], (double)states[4], (double)states[5]); + printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", (double)states[6], (double)states[7], (double)states[8]); exit(0); } else { From 8ad4b72fc76d702e7eac27c0aecfdb2885a8331d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 22:17:16 +0100 Subject: [PATCH 012/134] Compile fixes, publishing estimated attitude now --- .../fw_att_pos_estimator_main.cpp | 44 ++++++++++++++----- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1ed526e83f..5de97f9b93 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -577,20 +577,44 @@ FixedwingEstimator::task_main() if (_initialized) { math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Dcm R(q); + math::EulerAngles euler(R); - _att.q = q; + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = _gyro.timestamp; + _att.q[0] = states[0]; + _att.q[1] = states[1]; + _att.q[2] = states[2]; + _att.q[3] = states[3]; _att.q_valid = true; - _att.R_valid = false; + _att.R_valid = true; - // /* lazily publish the attitude only once available */ - // if (_att_pub > 0) { - // /* publish the attitude setpoint */ - // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + _att.timestamp = _gyro.timestamp; + _att.roll = euler.getPhi(); + _att.pitch = euler.getTheta(); + _att.yaw = euler.getPsi(); + // XXX find the right state + _att.rollspeed = _gyro.x - states[11]; + _att.pitchspeed = _gyro.y - states[12]; + _att.yawspeed = _gyro.z - states[13]; + // gyro offsets + _att.rate_offsets[0] = states[11]; + _att.rate_offsets[1] = states[12]; + _att.rate_offsets[2] = states[13]; - // } else { - // /* advertise and publish */ - // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - // } + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } + + _global_pos.timestamp = _gyro.timestamp; // /* lazily publish the position only once available */ // if (_global_pos_pub > 0) { From a5e95bde0a77cdd894b926860dba8bc98eaf4dbd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jan 2014 00:35:18 +0100 Subject: [PATCH 013/134] Added lots of instrumentation to ensure all data sources are coming in clean --- .../fw_att_pos_estimator_main.cpp | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index f25ab5ec03..ef51c7524c 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -145,6 +145,12 @@ private: struct mag_scale _mag_offsets; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _perf_gyro; /// 2) { /* fuse GPS updates */ @@ -529,6 +552,7 @@ FixedwingEstimator::task_main() orb_check(_mag_sub, &mag_updated); if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + perf_count(_perf_mag); // XXX we compensate the offsets upfront - should be close to zero. magData.x = _mag.x; @@ -555,6 +579,7 @@ FixedwingEstimator::task_main() orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated && _initialized) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { From f9e810d915ad53962e136f9f9e5cf285765fb59f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:56:04 +0100 Subject: [PATCH 014/134] Enable logging in HIL --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 181256dc71..6189d8fe3f 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -83,5 +83,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix fw_pos_control_l1 start fw_att_control start +sdlog2 start -r 50 -a -b 16 + echo "[HIL] setup done, running" From d8f04556beca666d1f932a289f2437cbe6b53958 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:56:24 +0100 Subject: [PATCH 015/134] Strip unused variable --- src/lib/geo/geo.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..ce2c64a59c 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -199,7 +199,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ From 8bd532c8556d7f99d1d66c161f182aeebd1ab325 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:57:01 +0100 Subject: [PATCH 016/134] Change slightly the way we handle timestamps, apply same unknown scaling as on logfiles --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index ef51c7524c..44c25c407a 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -84,10 +84,11 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); static uint64_t last_run = 0; +static uint32_t IMUmsec = 0; uint32_t millis() { - return last_run / 1e3; + return IMUmsec; } class FixedwingEstimator @@ -427,7 +428,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - float IMUmsec = _gyro.timestamp / 1e3; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -501,8 +502,6 @@ FixedwingEstimator::task_main() gpsLon = math::radians(_gps.lon / (double)1e7); gpsHgt = _gps.alt / 1e3f; - newDataGps = true; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); _initialized = true; @@ -540,12 +539,13 @@ FixedwingEstimator::task_main() fuseVelData = false; fusePosData = false; fuseHgtData = false; - - newDataGps = false; } } else { - newDataGps = false; + /* do not fuse anything, we got no position / vel update */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; } bool mag_updated; @@ -555,13 +555,15 @@ FixedwingEstimator::task_main() perf_count(_perf_mag); // XXX we compensate the offsets upfront - should be close to zero. - magData.x = _mag.x; + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _mag.x; magBias.x = 0.0f; // _mag_offsets.x_offset - magData.y = _mag.y; + magData.y = 0.001f * _mag.y; magBias.y = 0.0f; // _mag_offsets.y_offset - magData.z = _mag.z; + magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset if (_initialized) { From 395033eeb0b76eacef78d489a69af8442fadf947 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 09:47:49 +0100 Subject: [PATCH 017/134] Switch to 21 state version, publish local position as well --- .../fw_att_pos_estimator_main.cpp | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 44c25c407a..e6747604be 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -35,6 +35,8 @@ * @file fw_att_pos_estimator_main.cpp * Implementation of the attitude and position estimator. * + * @author Paul Riseborough + * @author Lorenz Meier */ #include @@ -55,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +132,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ + orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -139,6 +143,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct gyro_scale _gyro_offsets; @@ -227,6 +232,7 @@ FixedwingEstimator::FixedwingEstimator() : /* publications */ _att_pub(-1), _global_pos_pub(-1), + _local_pos_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), @@ -603,6 +609,15 @@ FixedwingEstimator::task_main() if (_initialized) { + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + math::Quaternion q(states[0], states[1], states[2], states[3]); math::Dcm R(q); math::EulerAngles euler(R); @@ -641,9 +656,33 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + _local_pos.timestamp = _gyro.timestamp; + _local_pos.x = states[7]; + _local_pos.y = states[8]; + _local_pos.z = states[9]; + + _local_pos.vx = states[4]; + _local_pos.vy = states[5]; + _local_pos.vz = states[6]; + + _local_pos.xy_valid = true; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = true; + _local_pos.v_z_valid = true; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + _global_pos.timestamp = _gyro.timestamp; - // /* lazily publish the position only once available */ + // /* lazily publish the global position only once available */ // if (_global_pos_pub > 0) { // /* publish the attitude setpoint */ // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); From e421260f7c3e2a3f7145c2f643f8440060a84777 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:57:53 +0100 Subject: [PATCH 018/134] Removed bogus sensor counters, replaced them with proper timestamps --- .../attitude_estimator_ekf_main.cpp | 14 ++++----- .../attitude_estimator_so3_main.cpp | 13 ++++---- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++--- src/modules/mavlink/orb_listener.c | 24 +++++++-------- .../position_estimator_inav_main.c | 16 +++++----- src/modules/sdlog2/sdlog2.c | 30 +++++++++---------- src/modules/sensors/sensors.cpp | 13 ++++---- src/modules/uORB/topics/sensor_combined.h | 17 +++++------ 8 files changed, 63 insertions(+), 75 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..1741fab8b9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -249,7 +249,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; @@ -333,9 +332,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -345,11 +343,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } z_k[3] = raw.accelerometer_m_s2[0]; @@ -357,11 +354,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613d..e2d69cd615 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -526,9 +526,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,11 +537,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,11 +548,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..37e5b8c616 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -388,7 +388,6 @@ handle_message(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = hil_counter; /* accelerometer */ static const float mg2ms2 = 9.8f / 1000.0f; @@ -400,7 +399,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.accelerometer_timestamp = hil_sensors.timestamp; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -418,17 +417,17 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_timestamp = hil_sensors.timestamp; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; + hil_sensors.baro_timestamp = hil_sensors.timestamp; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + hil_sensors.differential_pressure_timestamp = hil_sensors.timestamp; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; @@ -829,7 +828,7 @@ receive_thread(void *arg) while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + if (nread < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..4a4ebf709d 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -181,33 +181,33 @@ l_sensor_combined(const struct listener *l) /* mark individual fields as changed */ uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; + static hrt_abstime accel_counter = 0; + static hrt_abstime gyro_counter = 0; + static hrt_abstime mag_counter = 0; + static hrt_abstime baro_counter = 0; - if (accel_counter != raw.accelerometer_counter) { + if (accel_counter != raw.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; + accel_counter = raw.accelerometer_timestamp; } - if (gyro_counter != raw.gyro_counter) { + if (gyro_counter != raw.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; + gyro_counter = raw.timestamp; } - if (mag_counter != raw.magnetometer_counter) { + if (mag_counter != raw.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; + mag_counter = raw.magnetometer_timestamp; } - if (baro_counter != raw.baro_counter) { + if (baro_counter != raw.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; + baro_counter = raw.baro_timestamp; } if (gcs_link) 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 3084b6d928..1270ff5cf1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -177,8 +177,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime landed_time = 0; bool flag_armed = false; - uint32_t accel_counter = 0; - uint32_t baro_counter = 0; + hrt_abstime accel_timestamp = 0; + hrt_abstime baro_timestamp = 0; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -242,8 +242,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter != baro_counter) { - baro_counter = sensor.baro_counter; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { @@ -354,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter != accel_counter) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -376,13 +376,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(accel_corr, 0, sizeof(accel_corr)); } - accel_counter = sensor.accelerometer_counter; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - if (sensor.baro_counter != baro_counter) { + if (sensor.baro_timestamp != baro_timestamp) { baro_corr = - sensor.baro_alt_meter - z_est[0]; - baro_counter = sensor.baro_counter; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13cb..1b91374629 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -903,11 +903,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - uint16_t gyro_counter = 0; - uint16_t accelerometer_counter = 0; - uint16_t magnetometer_counter = 0; - uint16_t baro_counter = 0; - uint16_t differential_pressure_counter = 0; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; + hrt_abstime differential_pressure_timestamp = 0; /* enable logging on start if needed */ if (log_on_start) @@ -1003,28 +1003,28 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; + if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; write_IMU = true; } - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; + if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; write_IMU = true; } - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; + if (buf.sensor.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; write_SENS = true; } - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; + if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { + differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; write_SENS = true; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c271..a494cd08d6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_counter++; + raw.accelerometer_timestamp = accel_report.timestamp; } } @@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_counter++; + raw.timestamp = gyro_report.timestamp; } } @@ -987,7 +987,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_counter++; + raw.magnetometer_timestamp = mag_report.timestamp; } } @@ -1005,7 +1005,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_timestamp = _barometer.timestamp; } } @@ -1019,7 +1019,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + raw.differential_pressure_timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, @@ -1560,9 +1560,6 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); - /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..92812efd49 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** From cd05dfcc7cb557575e62f5f93ddd3a86c073d517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:58:17 +0100 Subject: [PATCH 019/134] Moved to sensor_combined topic for estimator --- .../fw_att_pos_estimator_main.cpp | 150 +++++++++++++++--- 1 file changed, 130 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e6747604be..466f6d51f4 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -50,9 +50,17 @@ #include #include #include + +#define SENSOR_COMBINED_SUB + + #include #include #include +#include +#ifdef SENSOR_COMBINED_SUB +#include +#endif #include #include #include @@ -60,14 +68,9 @@ #include #include #include -#include -#include #include -#include -#include #include #include -#include #include #include #include @@ -77,6 +80,8 @@ #include "../../../InertialNav/code/estimator.h" + + /** * estimator app start / stop handling function * @@ -118,12 +123,15 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _estimator_task; /**< task handle for sensor task */ - +#ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ + #else + int _sensor_combined_sub; + #endif int _airspeed_sub; /**< airspeed subscription */ + int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -135,12 +143,11 @@ private: orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ @@ -150,6 +157,10 @@ private: struct accel_scale _accel_offsets; struct mag_scale _mag_offsets; +#ifdef SENSOR_COMBINED_SUB + struct sensor_combined_s _sensor_combined; +#endif + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// 1.0f) + deltaT = 0.01f; + + // Always store data, independent of init status + /* fill in last data set */ + dtIMU = deltaT; + + angRate.x = _sensor_combined.gyro_rad_s[0]; + angRate.y = _sensor_combined.gyro_rad_s[1]; + angRate.z = _sensor_combined.gyro_rad_s[2]; + + accel.x = _sensor_combined.accelerometer_m_s2[0]; + accel.y = _sensor_combined.accelerometer_m_s2[1]; + accel.z = _sensor_combined.accelerometer_m_s2[2]; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + + if (last_mag != _sensor_combined.magnetometer_timestamp) { + mag_updated = true; + } + last_mag = _sensor_combined.magnetometer_timestamp; + + #endif + if (_initialized) { /* predict states and covariances */ @@ -490,6 +572,14 @@ FixedwingEstimator::task_main() } } + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + /* XXX leverage baro */ + } + bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { @@ -554,12 +644,16 @@ FixedwingEstimator::task_main() fuseHgtData = false; } - bool mag_updated; + #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); + #endif if (mag_updated) { - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + perf_count(_perf_mag); + #ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + // XXX we compensate the offsets upfront - should be close to zero. // XXX the purpose of the 0.001 factor is unclear // 0.001f @@ -571,6 +665,22 @@ FixedwingEstimator::task_main() magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset + + #else + + // XXX we compensate the offsets upfront - should be close to zero. + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _sensor_combined.magnetometer_ga[0]; + magBias.x = 0.0f; // _mag_offsets.x_offset + + magData.y = 0.001f * _sensor_combined.magnetometer_ga[1]; + magBias.y = 0.0f; // _mag_offsets.y_offset + + magData.z = 0.001f * _sensor_combined.magnetometer_ga[2]; + magBias.z = 0.0f; // _mag_offsets.y_offset + + #endif if (_initialized) { From bc64391c538d85e6a1640dac0a76fe0f71a8efc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:59:15 +0100 Subject: [PATCH 020/134] AUTOMATED code-style fix on topics. NO MANUAL OR SEMANTIC EDITS --- src/modules/uORB/topics/esc_status.h | 9 +- .../uORB/topics/filtered_bottom_flow.h | 3 +- src/modules/uORB/topics/home_position.h | 5 +- src/modules/uORB/topics/mission.h | 6 +- .../uORB/topics/navigation_capabilities.h | 2 +- .../uORB/topics/offboard_control_setpoint.h | 5 +- src/modules/uORB/topics/rc_channels.h | 55 ++++++----- src/modules/uORB/topics/subsystem_info.h | 3 +- src/modules/uORB/topics/telemetry_status.h | 22 ++--- .../uORB/topics/vehicle_attitude_setpoint.h | 3 +- .../topics/vehicle_bodyframe_speed_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_command.h | 91 +++++++++---------- .../uORB/topics/vehicle_control_debug.h | 9 +- .../uORB/topics/vehicle_control_mode.h | 5 +- .../uORB/topics/vehicle_global_position.h | 19 ++-- .../vehicle_global_position_set_triplet.h | 3 +- .../topics/vehicle_global_position_setpoint.h | 3 +- .../topics/vehicle_global_velocity_setpoint.h | 3 +- .../uORB/topics/vehicle_gps_position.h | 5 +- .../uORB/topics/vehicle_local_position.h | 3 +- .../topics/vehicle_local_position_setpoint.h | 3 +- .../uORB/topics/vehicle_rates_setpoint.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 49 +++++----- .../uORB/topics/vehicle_vicon_position.h | 3 +- 24 files changed, 148 insertions(+), 173 deletions(-) diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 11332d7a7a..628824efa8 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -60,7 +60,7 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { @@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. */ -struct esc_status_s -{ +struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - + uint8_t esc_count; /**< number of connected ESCs */ enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ - + struct { uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h index ab6de26131..c5d5455422 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -53,8 +53,7 @@ /** * Filtered bottom flow in bodyframe. */ -struct filtered_bottom_flow_s -{ +struct filtered_bottom_flow_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ float sumx; /**< Integrated bodyframe x flow in meters */ diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0fb..09f5140b37 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -53,11 +53,10 @@ /** * GPS home position in WGS84 coordinates. */ -struct home_position_s -{ +struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - + int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a5..04cbf73e11 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -67,8 +67,7 @@ enum NAV_CMD { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct mission_item_s -{ +struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees * 1E7 */ double lon; /**< longitude in degrees * 1E7 */ @@ -83,8 +82,7 @@ struct mission_item_s float param4; }; -struct mission_s -{ +struct mission_s { struct mission_item_s *items; unsigned count; }; diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3f..3a367b8cf6 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -52,7 +52,7 @@ * Airspeed */ struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ }; /** diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 7901b930af..68d3e494be 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -45,12 +45,11 @@ /** * Off-board control inputs. - * + * * Typically sent by a ground control station / joystick or by * some off-board controller via C or SIMULINK. */ -enum OFFBOARD_CONTROL_MODE -{ +enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef150..6eb20efd1b 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -52,29 +52,28 @@ */ #define RC_CHANNELS_MAPPED_MAX 15 -/** +/** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of * the channel array chan[]. */ -enum RC_CHANNELS_FUNCTION -{ - THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ +enum RC_CHANNELS_FUNCTION { + THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; /** @@ -85,16 +84,16 @@ enum RC_CHANNELS_FUNCTION struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ + uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ + struct { + float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ + } chan[RC_CHANNELS_MAPPED_MAX]; + uint8_t chan_count; /**< number of valid channels */ - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ + /*String array to store the names of the functions*/ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; + uint8_t rssi; /**< Overall receive signal strength */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index cfe0bf69e9..d3a7ce10d8 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,8 +50,7 @@ #include #include "../uORB.h" -enum SUBSYSTEM_TYPE -{ +enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, SUBSYSTEM_TYPE_ACC = 2, SUBSYSTEM_TYPE_MAG = 4, diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc7..c181ec04fb 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -44,10 +44,10 @@ #include "../uORB.h" enum TELEMETRY_STATUS_RADIO_TYPE { - TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, - TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, - TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, - TELEMETRY_STATUS_RADIO_TYPE_WIRE + TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, + TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, + TELEMETRY_STATUS_RADIO_TYPE_WIRE }; /** @@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ - uint8_t noise; /**< background noise level */ - uint8_t remote_noise; /**< remote background noise level */ - uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + unsigned remote_rssi; /**< remote signal strength */ + unsigned rxerrors; /**< receive errors */ + unsigned fixed; /**< count of error corrected packets */ + uint8_t noise; /**< background noise level */ + uint8_t remote_noise; /**< remote background noise level */ + uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a8..fda3cd75e6 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -52,8 +52,7 @@ /** * vehicle attitude setpoint. */ -struct vehicle_attitude_setpoint_s -{ +struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float roll_body; /**< body angle in NED frame */ diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h index fbfab09f30..fd1ade671a 100644 --- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -48,8 +48,7 @@ * @{ */ -struct vehicle_bodyframe_speed_setpoint_s -{ +struct vehicle_bodyframe_speed_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float vx; /**< in m/s */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6a..c21a29b130 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -51,43 +51,42 @@ * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, * but can contain additional ones. */ -enum VEHICLE_CMD -{ - VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END=501, /* | */ +enum VEHICLE_CMD { + VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END = 501, /* | */ }; /** @@ -96,14 +95,13 @@ enum VEHICLE_CMD * Should contain all of MAVLink's VEHICLE_CMD_RESULT values * but can contain additional ones. */ -enum VEHICLE_CMD_RESULT -{ - VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ - VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +enum VEHICLE_CMD_RESULT { + VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END = 5, /* | */ }; /** @@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT * @{ */ -struct vehicle_command_s -{ +struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h index 6184284a43..2a45eaccc2 100644 --- a/src/modules/uORB/topics/vehicle_control_debug.h +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_control_debug_s -{ +struct vehicle_control_debug_s { uint64_t timestamp; /**< in microseconds since system start */ float roll_p; /**< roll P control part */ @@ -77,9 +76,9 @@ struct vehicle_control_debug_s }; /**< vehicle_control_debug */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_control_debug); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b9..4c31f92b5f 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -38,7 +38,7 @@ /** * @file vehicle_control_mode.h * Definition of the vehicle_control_mode uORB topic. - * + * * All control apps should depend their actions based on the flags set here. */ @@ -59,8 +59,7 @@ * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_control_mode_s -{ +struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..4376fa1bab 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -51,16 +51,15 @@ * @{ */ - /** - * Fused global position in WGS84. - * - * This struct contains the system's believ about its position. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s -{ +/** +* Fused global position in WGS84. +* +* This struct contains the system's believ about its position. It is not the raw GPS +* measurement (@see vehicle_gps_position). This topic is usually published by the position +* estimator, which will take more sources of information into account than just GPS, +* e.g. control inputs of the vehicle in a Kalman-filter implementation. +*/ +struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 8516b263fc..e8565d887a 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -58,8 +58,7 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s -{ +struct vehicle_global_position_set_triplet_s { bool previous_valid; /**< flag indicating previous position is valid */ bool next_valid; /**< flag indicating next position is valid */ diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index a56434d3b0..8102e367da 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -58,8 +58,7 @@ * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct vehicle_global_position_setpoint_s -{ +struct vehicle_global_position_setpoint_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ int32_t lat; /**< latitude in degrees * 1E7 */ int32_t lon; /**< longitude in degrees * 1E7 */ diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h index 73961cdfed..5dac877d08 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -47,8 +47,7 @@ * @{ */ -struct vehicle_global_velocity_setpoint_s -{ +struct vehicle_global_velocity_setpoint_s { float vx; /**< in m/s NED */ float vy; /**< in m/s NED */ float vz; /**< in m/s NED */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 1639a08c2b..794c3f8bc7 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -53,13 +53,12 @@ /** * GPS position in WGS84 coordinates. */ -struct vehicle_gps_position_s -{ +struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ int32_t lat; /**< Latitude in 1E-7 degrees */ int32_t lon; /**< Longitude in 1E-7 degrees */ int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - + uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float p_variance_m; /**< position accuracy estimate m */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..44c339711d 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_local_position_s -{ +struct vehicle_local_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ bool xy_valid; /**< true if x and y are valid */ bool z_valid; /**< true if z is valid */ diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index d24d81e3a4..8988a0330d 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -49,8 +49,7 @@ * @{ */ -struct vehicle_local_position_setpoint_s -{ +struct vehicle_local_position_setpoint_s { float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 46e62c4b71..9f8b412a7d 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_rates_setpoint_s -{ +struct vehicle_rates_setpoint_s { uint64_t timestamp; /**< in microseconds since system start */ float roll; /**< body angular rates in NED frame */ @@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s }; /**< vehicle_rates_setpoint */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7a..7ac1a66604 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -131,31 +131,31 @@ enum VEHICLE_MODE_FLAG { * Should match 1:1 MAVLink's MAV_TYPE ENUM */ enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET=9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - VEHICLE_TYPE_KITE=17, /* Kite | */ - VEHICLE_TYPE_ENUM_END=18, /* | */ + VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ + VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ + VEHICLE_TYPE_KITE = 17, /* Kite | */ + VEHICLE_TYPE_ENUM_END = 18, /* | */ }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -168,8 +168,7 @@ enum VEHICLE_BATTERY_WARNING { * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_status_s -{ +struct vehicle_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ @@ -216,7 +215,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h index 0822fa89a1..e19a34a5d7 100644 --- a/src/modules/uORB/topics/vehicle_vicon_position.h +++ b/src/modules/uORB/topics/vehicle_vicon_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_vicon_position_s -{ +struct vehicle_vicon_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ bool valid; /**< true if position satisfies validity criteria of estimator */ From 6b55baad7853670165a93135416b6024397d9955 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 23:41:52 +0100 Subject: [PATCH 021/134] Tool command to print full filter states --- .../fw_att_pos_estimator_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 466f6d51f4..142abc50df 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -875,12 +875,16 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) math::Quaternion q(states[0], states[1], states[2], states[3]); math::EulerAngles euler(q); - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2]); - printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", (double)states[3], (double)states[4], (double)states[5]); - printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", (double)states[6], (double)states[7], (double)states[8]); + printf("states [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); + printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); + printf("states [20-21]: %8.4f, %8.4f\n", (double)states[19], (double)states[20]); exit(0); } else { From 8a37633d2d48dde675a2dda6d5c8f8af39197da0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jan 2014 08:32:44 +0100 Subject: [PATCH 022/134] Copyright cleanup --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a494cd08d6..e13fc4a919 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -37,6 +36,8 @@ * Sensor readout process. * * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include From 1bf7e10b9fc76327fa8d7322eca36ec4fc4935e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jan 2014 21:21:45 +0100 Subject: [PATCH 023/134] Better output --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 142abc50df..9ea4e93b82 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -878,9 +878,9 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); From b7b1c1428f672ccfec78e2e9ef4e2542a53f8821 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:52:40 +0100 Subject: [PATCH 024/134] Fixed perf init --- .../fw_att_pos_estimator_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9ea4e93b82..dc05540631 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -251,13 +251,13 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_ELAPSED, "fw_ekf_gyro_upd")), - _perf_accel(perf_alloc(PC_ELAPSED, "fw_ekf_accel_upd")), - _perf_mag(perf_alloc(PC_ELAPSED, "fw_ekf_mag_upd")), - _perf_gps(perf_alloc(PC_ELAPSED, "fw_ekf_gps_upd")), - _perf_baro(perf_alloc(PC_ELAPSED, "fw_ekf_baro_upd")), - _perf_airspeed(perf_alloc(PC_ELAPSED, "fw_ekf_aspd_upd")), + _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")), + _perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")), + _perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")), + _perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")), + _perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")), + _perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")), /* states */ _initialized(false) From fb446c01b815b00f5da098d340557164fdbf78c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jan 2014 14:39:01 +0100 Subject: [PATCH 025/134] Embedded estimator code locally --- .../fw_att_pos_estimator/estimator.cpp | 1843 +++++++++++++++++ src/modules/fw_att_pos_estimator/estimator.h | 172 ++ .../fw_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_pos_estimator/module.mk | 2 +- 4 files changed, 2017 insertions(+), 2 deletions(-) create mode 100644 src/modules/fw_att_pos_estimator/estimator.cpp create mode 100644 src/modules/fw_att_pos_estimator/estimator.h diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp new file mode 100644 index 0000000000..388cea8646 --- /dev/null +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -0,0 +1,1843 @@ +#include "estimator.h" + +// Global variables +float KH[n_states][n_states]; // intermediate result used for covariance updates +float KHP[n_states][n_states]; // intermediate result used for covariance updates +float P[n_states][n_states]; // covariance matrix +float Kfusion[n_states]; // Kalman gains +float states[n_states]; // state matrix +float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps +uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored +Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) +Vector3f dVelIMU; +Vector3f dAngIMU; +float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) +float dt; // time lapsed since last covariance prediction +bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) +bool useAirspeed = true; // boolean true if airspeed data is being used +bool useCompass = true; // boolean true if magnetometer data is being used +uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity +float innovVelPos[6]; // innovation output +float varInnovVelPos[6]; // innovation variance output +bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused +bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused +bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused +bool fuseMagData = false; // boolean true when magnetometer data is to be fused + +float velNED[3]; // North, East, Down velocity obs (m/s) +float posNE[2]; // North, East position obs (m) +float hgtMea; // measured height (m) +float posNED[3]; // North, East Down position (m) + +float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement +float innovMag[3]; // innovation output +float varInnovMag[3]; // innovation variance output +Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +float innovVtas; // innovation output +float varInnovVtas; // innovation variance output +bool fuseVtasData = false; // boolean true when airspeed data is to be fused +float VtasMeas; // true airspeed measurement (m/s) +float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +float latRef; // WGS-84 latitude of reference point (rad) +float lonRef; // WGS-84 longitude of reference point (rad) +float hgtRef; // WGS-84 height of reference point (m) +Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + +// GPS input data variables +float gpsCourse; +float gpsGndSpd; +float gpsVelD; +float gpsLat; +float gpsLon; +float gpsHgt; +uint8_t GPSstatus; + + +bool statesInitialised = false; + + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +Vector3f Vector3f::zero(void) const +{ + Vector3f ret = *this; + ret.x = 0.0; + ret.y = 0.0; + ret.z = 0.0; + return ret; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} + +void UpdateStrapdownEquationsNED() +{ + static Vector3f prevDelAng; + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + static Mat3f Tbn; + static Mat3f Tnb; + float rotationMag; + float rotScaler; + float qUpdated[4]; + float quatMag; + float quatMagInv; + float deltaQuat[4]; + static float lastVelocity[3]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + + // Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z; + + // Save current measurements + prevDelAng = correctedDelAng; + + // Apply corrections for earths rotation rate and coning errors + // * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + + // Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + + // Update the quaternions by rotating from the previous attitude through + // the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + + // Normalise the quaternions and update the quaternion states + quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + + // Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + + // transform body delta velocities to delta velocities in the nav frame + // * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + + // calculate the magnitude of the nav acceleration (required for GPS + // variance estimation) + accNavMag = delVelNav.length()/dtIMU; + + // If calculating position save previous velocity + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + + // Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + + // If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + +} + +void CovariancePrediction() +{ + // scalars + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + + // arrays + float processNoise[n_states]; + float SF[14]; + float SG[8]; + float SQ[11]; + float SPP[13]; + float nextP[n_states][n_states]; + + // calculate covariance prediction process noise + const float yawVarScale = 1.0f; + windVelSigma = dt*0.1f; + dAngBiasSigma = dt*5.0e-7f; + magEarthSigma = dt*3.0e-4f; + magBodySigma = dt*3.0e-4f; + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; + for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; + for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; + for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + daxCov = sq(dt*1.4544411e-2f); + dayCov = sq(dt*1.4544411e-2f); + dazCov = sq(dt*1.4544411e-2f); + if (onGround) dazCov = dazCov * sq(yawVarScale); + dvxCov = sq(dt*0.5f); + dvyCov = sq(dt*0.5f); + dvzCov = sq(dt*0.5f); + + // Predicted covariance calculation + SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; + SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SF[3] = day/2 - day_b/2; + SF[4] = daz/2 - daz_b/2; + SF[5] = dax/2 - dax_b/2; + SF[6] = dax_b/2 - dax/2; + SF[7] = daz_b/2 - daz/2; + SF[8] = day_b/2 - day/2; + SF[9] = q1/2; + SF[10] = q2/2; + SF[11] = q3/2; + SF[12] = 2*dvz*q0; + SF[13] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; + SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SPP[3] = SF[11]; + SPP[4] = SF[10]; + SPP[5] = SF[9]; + SPP[6] = SF[7]; + SPP[7] = SF[8]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); + nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); + nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); + nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); + nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); + nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); + nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; + nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; + nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; + nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; + nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; + nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; + nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; + nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; + nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; + nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; + nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); + nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); + nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); + nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); + nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); + nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; + nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; + nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; + nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; + nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; + nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; + nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; + nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; + nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; + nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; + nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; + nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); + nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); + nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); + nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); + nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); + nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); + nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; + nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; + nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; + nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; + nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; + nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; + nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; + nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; + nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; + nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; + nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; + nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); + nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); + nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); + nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); + nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); + nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; + nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; + nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; + nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; + nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; + nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; + nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; + nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; + nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; + nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; + nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; + nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; + nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; + nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; + nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; + nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; + nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; + nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; + nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; + nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; + nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; + nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; + nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; + nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; + nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; + nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; + nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; + nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; + nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; + nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; + nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; + nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; + nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; + nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; + nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; + nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; + nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; + nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; + nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; + nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; + nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; + nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; + nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; + nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + + for (uint8_t i=0; i< n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,15,20); + zeroCols(nextP,15,20); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,13,14); + zeroCols(nextP,13,14); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (uint8_t j=0; j<=20; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (uint8_t i=0; i< n_states; i++) P[i][i] = nextP[i][i]; + for (uint8_t i=1; i< n_states; i++) + { + for (uint8_t j=0; j<=i-1; j++) + { + P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + // +} + +void FuseVelposNED() +{ + + // declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + static uint32_t velFailTime; + static uint32_t posFailTime; + static uint32_t hgtFailTime; + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + + // declare variables used to check measurement errors + float velInnov[3] = {0.0,0.0,0.0}; + float posInnov[2] = {0.0,0.0}; + float hgtInnov = 0.0; + + // declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + unsigned obsIndex; + unsigned indexLimit; + + // declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + + // Perform sequential fusion of GPS measurements. This assumes that the + // errors in the different velocity and position components are + // uncorrelated which is not true, however in the absence of covariance + // data from the GPS receiver it is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = 0.04f + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = 0.08f + sq(velErr); + R_OBS[3] = R_OBS[2]; + R_OBS[4] = 4.0f + sq(posErr); + R_OBS[5] = 4.0f; + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + velTimeout = (millis() - velFailTime) > horizRetryTime; + if (velHealth || velTimeout) + { + velHealth = true; + velFailTime = millis(); + } + else + { + velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + posTimeout = (millis() - posFailTime) > horizRetryTime; + if (posHealth || posTimeout) + { + posHealth = true; + posFailTime = millis(); + } + else + { + posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime; + if (hgtHealth || hgtTimeout) + { + hgtHealth = true; + hgtFailTime = millis(); + } + else + { + hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } +} + +void FuseMagnetometer() +{ + + static float q0 = 1.0; + static float q1 = 0.0; + static float q2 = 0.0; + static float q3 = 0.0; + static float magN = 0.0; + static float magE = 0.0; + static float magD = 0.0; + static float magXbias = 0.0; + static float magYbias = 0.0; + static float magZbias = 0.0; + static unsigned obsIndex; + uint8_t indexLimit; + float DCM[3][3] = + { + {1.0,0.0,0.0} , + {0.0,1.0,0.0} , + {0.0,0.0,1.0} + }; + static float MagPred[3] = {0.0,0.0,0.0}; + static float R_MAG; + static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + float H_MAG[n_states]; + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + + // Perform sequential fusion of Magnetometer measurements. + // This assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[15]; + magE = statesAtMagMeasTime[16]; + magD = statesAtMagMeasTime[17]; + magXbias = statesAtMagMeasTime[18]; + magYbias = statesAtMagMeasTime[19]; + magZbias = statesAtMagMeasTime[20]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM[0][1] = 2*(q1*q2 + q0*q3); + DCM[0][2] = 2*(q1*q3-q0*q2); + DCM[1][0] = 2*(q1*q2 - q0*q3); + DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM[1][2] = 2*(q2*q3 + q0*q1); + DCM[2][0] = 2*(q1*q3 + q0*q2); + DCM[2][1] = 2*(q2*q3 - q0*q1); + DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; + MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; + MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[16] = 2*q0*q3 + 2*q1*q2; + H_MAG[17] = 2*q1*q3 - 2*q0*q2; + H_MAG[18] = 1; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); + Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[15] = 2*q1*q2 - 2*q0*q3; + H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[17] = 2*q0*q1 + 2*q2*q3; + H_MAG[19] = 1; + + // Calculate Kalman gain + SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[15] = 2*q0*q2 + 2*q1*q3; + H_MAG[16] = 2*q2*q3 - 2*q0*q1; + H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[20] = 1; + + // Calculate Kalman gain + SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); + Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k<=3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 15; k<=20; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; +} + +void FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_TAS = 2.0f; + float SH_TAS[3]; + float SK_TAS; + float H_TAS[n_states]; + float Kfusion[n_states]; + float VtasPred; + float quatMag; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[13]; + vwe = statesAtVtasMeasTime[14]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; + SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; + for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[13] = -SH_TAS[2]; + H_TAS[14] = -SH_TAS[1]; + + // Calculate Kalman gains + SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j<=20; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in H to reduce the + // number of operations + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j<=6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; + for (uint8_t j = 13; j<=14; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k<=6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 13; k<=14; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } +} +void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col +#include + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0f +#define earthRadiusInv 1.5678540e-7f + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + Vector3f zero(void) const; +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +const unsigned int n_states = 21; +const unsigned int data_buffer_size = 50; + +extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored +extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) +extern Vector3f dVelIMU; +extern Vector3f dAngIMU; + +extern float P[n_states][n_states]; // covariance matrix +extern float Kfusion[n_states]; // Kalman gains +extern float states[n_states]; // state matrix +extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + +extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + +extern float dt; // time lapsed since last covariance prediction +extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + +extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) +extern bool useAirspeed; // boolean true if airspeed data is being used +extern bool useCompass; // boolean true if magnetometer data is being used +extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity +extern float innovVelPos[6]; // innovation output +extern float varInnovVelPos[6]; // innovation variance output + +extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused +extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused +extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused +extern bool fuseMagData; // boolean true when magnetometer data is to be fused + +extern float velNED[3]; // North, East, Down velocity obs (m/s) +extern float posNE[2]; // North, East position obs (m) +extern float hgtMea; // measured height (m) +extern float posNED[3]; // North, East Down position (m) + +extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + +extern float innovMag[3]; // innovation output +extern float varInnovMag[3]; // innovation variance output +extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +extern float innovVtas; // innovation output +extern float varInnovVtas; // innovation variance output +extern bool fuseVtasData; // boolean true when airspeed data is to be fused +extern float VtasMeas; // true airspeed measurement (m/s) +extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +extern float latRef; // WGS-84 latitude of reference point (rad) +extern float lonRef; // WGS-84 longitude of reference point (rad) +extern float hgtRef; // WGS-84 height of reference point (m) +extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +extern float EAS2TAS; // ratio f true to equivalent airspeed + +// GPS input data variables +extern float gpsCourse; +extern float gpsGndSpd; +extern float gpsVelD; +extern float gpsLat; +extern float gpsLon; +extern float gpsHgt; +extern uint8_t GPSstatus; + +extern bool statesInitialised; + +const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions +const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last); + +void zeroCols(float covMat[n_states][n_states], uint8_t first, uint8_t last); + +float sq(float valIn); + +void quatNorm(float quatOut[4], float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(); + +// recall stste vector stored at closest time to the one specified by msec +void RecallStates(float statesForFusion[n_states], uint32_t msec); + +void quat2Tnb(Mat3f &Tnb, float quat[4]); + +void quat2Tbn(Mat3f &Tbn, float quat[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +void eul2quat(float quat[4], float eul[3]); + +void quat2eul(float eul[3],float quat[4]); + +void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(); + +uint32_t millis(); + diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index dc05540631..7a71e774c8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -78,7 +78,7 @@ #include #include -#include "../../../InertialNav/code/estimator.h" +#include "estimator.h" diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk index c1384be7e9..c992959e0d 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -39,4 +39,4 @@ MODULE_COMMAND = fw_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ - ../../../../InertialNav/code/estimator.cpp + estimator.cpp From a937e972b059bfc6f6ec99fd5bcf064d89d29acb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 09:53:48 +0100 Subject: [PATCH 026/134] Updated to latest estimator version --- .../fw_att_pos_estimator/estimator.cpp | 218 +++++++++--------- src/modules/fw_att_pos_estimator/estimator.h | 3 + 2 files changed, 114 insertions(+), 107 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 388cea8646..c7c9b6476d 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -20,16 +20,9 @@ Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) float dt; // time lapsed since last covariance prediction -bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) -bool useAirspeed = true; // boolean true if airspeed data is being used -bool useCompass = true; // boolean true if magnetometer data is being used uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output -bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused -bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused -bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused -bool fuseMagData = false; // boolean true when magnetometer data is to be fused float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) @@ -45,7 +38,6 @@ Vector3f magData; // magnetometer flux radings in X,Y,Z body axes float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float innovVtas; // innovation output float varInnovVtas; // innovation variance output -bool fuseVtasData = false; // boolean true when airspeed data is to be fused float VtasMeas; // true airspeed measurement (m/s) float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) @@ -64,9 +56,20 @@ float gpsLon; float gpsHgt; uint8_t GPSstatus; +// Baro input +float baroHgt; bool statesInitialised = false; +bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused +bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused +bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused +bool fuseMagData = false; // boolean true when magnetometer data is to be fused +bool fuseVtasData = false; // boolean true when airspeed data is to be fused + +bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) +bool useAirspeed = true; // boolean true if airspeed data is being used +bool useCompass = true; // boolean true if magnetometer data is being used float Vector3f::length(void) const { @@ -182,23 +185,23 @@ void UpdateStrapdownEquationsNED() float deltaQuat[4]; static float lastVelocity[3]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - - // Remove sensor bias errors + +// Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z; - - // Save current measurements + +// Save current measurements prevDelAng = correctedDelAng; - - // Apply corrections for earths rotation rate and coning errors - // * and + operators have been overloaded + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - - // Convert the rotation vector to its equivalent quaternion + +// Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -215,15 +218,15 @@ void UpdateStrapdownEquationsNED() deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; } - - // Update the quaternions by rotating from the previous attitude through - // the delta angle rotation quaternion + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - - // Normalise the quaternions and update the quaternion states + +// Normalise the quaternions and update the quaternion states quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { @@ -233,8 +236,8 @@ void UpdateStrapdownEquationsNED() states[2] = quatMagInv*qUpdated[2]; states[3] = quatMagInv*qUpdated[3]; } - - // Calculate the body to nav cosine matrix + +// Calculate the body to nav cosine matrix q00 = sq(states[0]); q11 = sq(states[1]); q22 = sq(states[2]); @@ -245,7 +248,7 @@ void UpdateStrapdownEquationsNED() q12 = states[1]*states[2]; q13 = states[1]*states[3]; q23 = states[2]*states[3]; - + Tbn.x.x = q00 + q11 - q22 - q33; Tbn.y.y = q00 - q11 + q22 - q33; Tbn.z.z = q00 - q11 - q22 + q33; @@ -255,35 +258,35 @@ void UpdateStrapdownEquationsNED() Tbn.y.z = 2*(q23 - q01); Tbn.z.x = 2*(q13 - q02); Tbn.z.y = 2*(q23 + q01); - + Tnb = Tbn.transpose(); - - // transform body delta velocities to delta velocities in the nav frame - // * and + operators have been overloaded + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - - // calculate the magnitude of the nav acceleration (required for GPS - // variance estimation) + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) accNavMag = delVelNav.length()/dtIMU; - - // If calculating position save previous velocity + +// If calculating position save previous velocity lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; - - // Sum delta velocities to get velocity + +// Sum delta velocities to get velocity states[4] = states[4] + delVelNav.x; states[5] = states[5] + delVelNav.y; states[6] = states[6] + delVelNav.z; - - // If calculating postions, do a trapezoidal integration for position + +// If calculating postions, do a trapezoidal integration for position states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - + } void CovariancePrediction() @@ -313,15 +316,15 @@ void CovariancePrediction() float dax_b; float day_b; float daz_b; - + // arrays - float processNoise[n_states]; + float processNoise[21]; float SF[14]; float SG[8]; float SQ[11]; float SPP[13]; - float nextP[n_states][n_states]; - + float nextP[21][21]; + // calculate covariance prediction process noise const float yawVarScale = 1.0f; windVelSigma = dt*0.1f; @@ -335,7 +338,7 @@ void CovariancePrediction() for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - + // set variables used to calculate covariance growth dvx = summedDelVel.x; dvy = summedDelVel.y; @@ -357,7 +360,7 @@ void CovariancePrediction() dvxCov = sq(dt*0.5f); dvyCov = sq(dt*0.5f); dvzCov = sq(dt*0.5f); - + // Predicted covariance calculation SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; @@ -373,7 +376,7 @@ void CovariancePrediction() SF[11] = q3/2; SF[12] = 2*dvz*q0; SF[13] = 2*dvy*q1; - + SG[0] = q0/2; SG[1] = sq(q3); SG[2] = sq(q2); @@ -382,7 +385,7 @@ void CovariancePrediction() SG[5] = 2*q2*q3; SG[6] = 2*q1*q3; SG[7] = 2*q1*q2; - + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); @@ -394,7 +397,7 @@ void CovariancePrediction() SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; SQ[9] = sq(SG[0]); SQ[10] = sq(q1); - + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; @@ -403,7 +406,7 @@ void CovariancePrediction() SPP[5] = SF[9]; SPP[6] = SF[7]; SPP[7] = SF[8]; - + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; @@ -845,12 +848,12 @@ void CovariancePrediction() nextP[20][18] = P[20][18]; nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; - - for (uint8_t i=0; i< n_states; i++) + + for (uint8_t i=0; i<= 20; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } - + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by // setting the coresponding covariance terms to zero. if (onGround || !useCompass) @@ -858,7 +861,7 @@ void CovariancePrediction() zeroRows(nextP,15,20); zeroCols(nextP,15,20); } - + // If on ground or not using airspeed sensing, inhibit wind velocity // covariance growth. if (onGround || !useAirspeed) @@ -866,7 +869,7 @@ void CovariancePrediction() zeroRows(nextP,13,14); zeroCols(nextP,13,14); } - + // If the total position variance exceds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -882,11 +885,11 @@ void CovariancePrediction() } } } - + // Force symmetry on the covariance matrix to prevent ill-conditioning // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i< n_states; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i< n_states; i++) + for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; + for (uint8_t i=1; i<=20; i++) { for (uint8_t j=0; j<=i-1; j++) { @@ -894,14 +897,14 @@ void CovariancePrediction() P[j][i] = P[i][j]; } } - + // } void FuseVelposNED() { - - // declare variables used by fault isolation logic + +// declare variables used by fault isolation logic uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time @@ -915,43 +918,43 @@ void FuseVelposNED() bool velTimeout; bool posTimeout; bool hgtTimeout; - - // declare variables used to check measurement errors + +// declare variables used to check measurement errors float velInnov[3] = {0.0,0.0,0.0}; float posInnov[2] = {0.0,0.0}; float hgtInnov = 0.0; - - // declare variables used to control access to arrays + +// declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; - unsigned obsIndex; - unsigned indexLimit; - - // declare variables used by state and covariance update calculations + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations float velErr; float posErr; float R_OBS[6]; float observation[6]; float SK; float quatMag; - - // Perform sequential fusion of GPS measurements. This assumes that the - // errors in the different velocity and position components are - // uncorrelated which is not true, however in the absence of covariance - // data from the GPS receiver it is the only assumption we can make - // so we might as well take advantage of the computational efficiencies - // associated with sequential fusion + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { // set the GPS data timeout depending on whether airspeed data is present if (useAirspeed) horizRetryTime = gpsRetryTime; else horizRetryTime = gpsRetryTimeNoTAS; - + // Form the observation vector for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; observation[5] = -(hgtMea); - + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring @@ -961,7 +964,7 @@ void FuseVelposNED() R_OBS[3] = R_OBS[2]; R_OBS[4] = 4.0f + sq(posErr); R_OBS[5] = 4.0f; - + // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) { @@ -1069,7 +1072,7 @@ void FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex <= 2) + if (obsIndex >= 0 && obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } @@ -1126,7 +1129,7 @@ void FuseVelposNED() void FuseMagnetometer() { - + static float q0 = 1.0; static float q1 = 0.0; static float q2 = 0.0; @@ -1137,7 +1140,7 @@ void FuseMagnetometer() static float magXbias = 0.0; static float magYbias = 0.0; static float magZbias = 0.0; - static unsigned obsIndex; + static uint8_t obsIndex; uint8_t indexLimit; float DCM[3][3] = { @@ -1148,17 +1151,17 @@ void FuseMagnetometer() static float MagPred[3] = {0.0,0.0,0.0}; static float R_MAG; static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - float H_MAG[n_states]; + float H_MAG[21]; float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - - // Perform sequential fusion of Magnetometer measurements. - // This assumes that the errors in the different components are - // uncorrelated which is not true, however in the absence of covariance - // data fit is the only assumption we can make - // so we might as well take advantage of the computational efficiencies - // associated with sequential fusion + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) { // Limit range of states modified when on ground @@ -1170,10 +1173,10 @@ void FuseMagnetometer() { indexLimit = 12; } - + // Sequential fusion of XYZ components to spread processing load across // three prediction time steps. - + // Calculate observation jacobians and Kalman gains if (fuseMagData) { @@ -1188,7 +1191,7 @@ void FuseMagnetometer() magXbias = statesAtMagMeasTime[18]; magYbias = statesAtMagMeasTime[19]; magZbias = statesAtMagMeasTime[20]; - + // rotate predicted earth components into body axes and calculate // predicted measurments DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; @@ -1203,10 +1206,10 @@ void FuseMagnetometer() MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - + // scale magnetometer observation error with total angular rate R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - + // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; @@ -1226,7 +1229,7 @@ void FuseMagnetometer() H_MAG[16] = 2*q0*q3 + 2*q1*q2; H_MAG[17] = 2*q1*q3 - 2*q0*q2; H_MAG[18] = 1; - + // Calculate Kalman gain SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; @@ -1257,7 +1260,7 @@ void FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - + // reset the observation index to 0 (we start by fusing the X // measurement) obsIndex = 0; @@ -1274,7 +1277,7 @@ void FuseMagnetometer() H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; H_MAG[17] = 2*q0*q1 + 2*q2*q3; H_MAG[19] = 1; - + // Calculate Kalman gain SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; @@ -1317,7 +1320,7 @@ void FuseMagnetometer() H_MAG[16] = 2*q2*q3 - 2*q0*q1; H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[20] = 1; - + // Calculate Kalman gain SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; @@ -1348,9 +1351,9 @@ void FuseMagnetometer() Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; - + } - + // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) { @@ -1434,18 +1437,18 @@ void FuseAirspeed() const float R_TAS = 2.0f; float SH_TAS[3]; float SK_TAS; - float H_TAS[n_states]; - float Kfusion[n_states]; + float H_TAS[21]; + float Kfusion[21]; float VtasPred; float quatMag; - + // Copy required states to local variable names vn = statesAtVtasMeasTime[4]; ve = statesAtVtasMeasTime[5]; vd = statesAtVtasMeasTime[6]; vwn = statesAtVtasMeasTime[13]; vwe = statesAtVtasMeasTime[14]; - + // Need to check that it is flying before fusing airspeed data // Calculate the predicted airspeed VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); @@ -1462,7 +1465,7 @@ void FuseAirspeed() H_TAS[6] = vd*SH_TAS[0]; H_TAS[13] = -SH_TAS[2]; H_TAS[14] = -SH_TAS[1]; - + // Calculate Kalman gains SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); @@ -1487,7 +1490,7 @@ void FuseAirspeed() Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); varInnovVtas = 1.0f/SK_TAS; - + // Calculate the measurement innovation innovVtas = VtasPred - VtasMeas; // Check the innovation for consistency and don't fuse if > 5Sigma @@ -1550,6 +1553,7 @@ void FuseAirspeed() } } } + void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1598,7 +1602,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec) long int bestTimeDelta = 200; uint8_t storeIndex; uint8_t bestStoreIndex = 0; - for (storeIndex=0; storeIndex < n_states; storeIndex++) + for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++) { timeDelta = msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d95745c80c..a7dd08a935 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -117,6 +117,9 @@ extern float gpsLon; extern float gpsHgt; extern uint8_t GPSstatus; +// Baro input +extern float baroHgt; + extern bool statesInitialised; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions From e32d92e7322fef0acdfb8913c930d90abfbfa627 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 10:00:03 +0100 Subject: [PATCH 027/134] =?UTF-8?q?Switch=20to=20Paul=E2=80=99s=20estimato?= =?UTF-8?q?r=20for=20all=20fixed=20wing=20setups?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index d354fb06fa..9ab310ab9e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Start attitude controller From 9695ae8cee414429c782403ffaa8ca0c9d90dc2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 10:00:26 +0100 Subject: [PATCH 028/134] Update API to include baro altitude. --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7a71e774c8..88c75903e2 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -400,6 +400,7 @@ FixedwingEstimator::task_main() fuseHgtData = false; fuseMagData = false; fuseVtasData = false; + statesInitialised = false; /* wakeup source(s) */ struct pollfd fds[2]; @@ -577,7 +578,7 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - /* XXX leverage baro */ + baroHgt = _baro.altitude; } bool gps_updated; @@ -601,6 +602,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); _initialized = true; + warnx("init done."); continue; } From d4ccd9bc45d82dff38b19848ab88eaae35853b94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 14:08:27 +0100 Subject: [PATCH 029/134] Fix a number of interface scaling / offset stupidities, should be closer to operational now on HW. --- .../fw_att_pos_estimator_main.cpp | 69 +++++++++++-------- 1 file changed, 39 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 88c75903e2..10f7d42acd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -596,7 +596,7 @@ FixedwingEstimator::task_main() gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); gpsVelD = _gps.vel_d_m_s; gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7); + gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { @@ -610,7 +610,9 @@ FixedwingEstimator::task_main() if (_initialized) { /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); posNE[0] = posNED[0]; @@ -657,30 +659,28 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. - // XXX the purpose of the 0.001 factor is unclear // 0.001f - magData.x = 0.001f * _mag.x; - magBias.x = 0.0f; // _mag_offsets.x_offset + magData.x = _mag.x; + magBias.x = 0.0001f; // _mag_offsets.x_offset - magData.y = 0.001f * _mag.y; - magBias.y = 0.0f; // _mag_offsets.y_offset + magData.y = _mag.y; + magBias.y = 0.0001f; // _mag_offsets.y_offset - magData.z = 0.001f * _mag.z; - magBias.z = 0.0f; // _mag_offsets.y_offset + magData.z = _mag.z; + magBias.z = 0.0001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. - // XXX the purpose of the 0.001 factor is unclear // 0.001f - magData.x = 0.001f * _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.0f; // _mag_offsets.x_offset + magData.x = _sensor_combined.magnetometer_ga[0]; + magBias.x = 0.0001f; // _mag_offsets.x_offset - magData.y = 0.001f * _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.0f; // _mag_offsets.y_offset + magData.y = _sensor_combined.magnetometer_ga[1]; + magBias.y = 0.0001f; // _mag_offsets.y_offset - magData.z = 0.001f * _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.0f; // _mag_offsets.y_offset + magData.z = _sensor_combined.magnetometer_ga[2]; + magBias.z = 0.0001f; // _mag_offsets.y_offset #endif @@ -749,14 +749,14 @@ FixedwingEstimator::task_main() _att.roll = euler.getPhi(); _att.pitch = euler.getTheta(); _att.yaw = euler.getPsi(); - // XXX find the right state - _att.rollspeed = _gyro.x - states[11]; - _att.pitchspeed = _gyro.y - states[12]; - _att.yawspeed = _gyro.z - states[13]; + + _att.rollspeed = angRate.x - states[10]; + _att.pitchspeed = angRate.y - states[11]; + _att.yawspeed = angRate.z - states[12]; // gyro offsets - _att.rate_offsets[0] = states[11]; - _att.rate_offsets[1] = states[12]; - _att.rate_offsets[2] = states[13]; + _att.rate_offsets[0] = states[10]; + _att.rate_offsets[1] = states[11]; + _att.rate_offsets[2] = states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -880,13 +880,22 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); - printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); - printf("states [20-21]: %8.4f, %8.4f\n", (double)states[19], (double)states[20]); + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); exit(0); } else { From 40a3510e75733d240265b05afff9002787d6067a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 14:11:45 +0100 Subject: [PATCH 030/134] Fix estimator timestamp handling for the two interface cases --- .../fw_att_pos_estimator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 88c75903e2..88684588c5 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -456,6 +456,8 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + hrt_abstime last_sensor_timestamp; + /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -468,7 +470,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - + last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; @@ -510,7 +512,7 @@ FixedwingEstimator::task_main() // Copy gyro and accel - + last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; @@ -737,7 +739,7 @@ FixedwingEstimator::task_main() for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.q[0] = states[0]; _att.q[1] = states[1]; _att.q[2] = states[2]; @@ -745,7 +747,7 @@ FixedwingEstimator::task_main() _att.q_valid = true; _att.R_valid = true; - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.roll = euler.getPhi(); _att.pitch = euler.getTheta(); _att.yaw = euler.getPsi(); @@ -768,7 +770,7 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } - _local_pos.timestamp = _gyro.timestamp; + _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; _local_pos.z = states[9]; From 8e35f6727c20960c05c6e7fad38fccb80f0bdc5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 16:52:54 +0100 Subject: [PATCH 031/134] Testing missing init bit in array initialization --- src/modules/fw_att_pos_estimator/estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c7c9b6476d..9b57dfd550 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1730,6 +1730,7 @@ void calcEarthRateNED(Vector3f &omega, float latitude) void CovarianceInit() { // Calculate the initial covariance matrix P + P[0][0] = 0.25f*sq(1.0f*deg2rad); P[1][1] = 0.25f*sq(1.0f*deg2rad); P[2][2] = 0.25f*sq(1.0f*deg2rad); P[3][3] = 0.25f*sq(10.0f*deg2rad); From 9cb59e33a6da2bcd2bb49bc3d49ce527597582f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 17:43:49 +0100 Subject: [PATCH 032/134] Initializing variables --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 10f7d42acd..3ed8163455 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -392,8 +392,6 @@ FixedwingEstimator::task_main() parameters_update(); - Vector3f lastAngRate; - Vector3f lastAccel; /* set initial filter state */ fuseVelData = false; fusePosData = false; @@ -402,6 +400,11 @@ FixedwingEstimator::task_main() fuseVtasData = false; statesInitialised = false; + /* initialize measurement data */ + VtasMeas = 0.0f; + Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + /* wakeup source(s) */ struct pollfd fds[2]; From 70ffa27acd6b5cd276e45d8c029ea743c32b2bc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 12:09:54 +0100 Subject: [PATCH 033/134] Rewrote the filter mainloop to match the order in the offboard simulator, added a number of scaling fixes, initializing all structs correctly --- .../fw_att_pos_estimator/estimator.cpp | 49 +++- src/modules/fw_att_pos_estimator/estimator.h | 7 +- .../fw_att_pos_estimator_main.cpp | 237 ++++++++++-------- 3 files changed, 167 insertions(+), 126 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9b57dfd550..3373319d0c 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,8 @@ #include "estimator.h" +// For debugging only +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m) float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement +float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float innovVtas; // innovation output float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) -float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -1125,6 +1129,8 @@ void FuseVelposNED() } } } + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } void FuseMagnetometer() @@ -1586,13 +1592,15 @@ float sq(float valIn) } // Store states in a history array along with time stamp -void StoreStates() +void StoreStates(uint64_t timestamp_ms) { static uint8_t storeIndex = 0; - if (storeIndex == data_buffer_size) storeIndex = 0; - for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i]; - statetimeStamp[storeIndex] = millis(); - storeIndex = storeIndex + 1; + for (unsigned i=0; i 1.0f) + if (deltaT > 1.0f || deltaT < 0.000001f) deltaT = 0.01f; // Always store data, independent of init status @@ -549,41 +556,13 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - /* predict states and covariances */ - - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); - - /* store the predictions */ - StoreStates(); - - /* evaluate if on ground */ - OnGroundCheck(); - - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; - - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } - } - - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - - baroHgt = _baro.altitude; + VtasMeas = _airspeed.true_airspeed_m_s; } bool gps_updated; @@ -592,65 +571,36 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); - if (_gps.fix_type > 2) { + if (_gps.fix_type < 3) { + gps_updated = false; + } else { /* fuse GPS updates */ //_gps.timestamp / 1e3; GPSstatus = _gps.fix_type; - gpsCourse = _gps.cog_rad; - gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); - gpsVelD = _gps.vel_d_m_s; + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { - InitialiseFilter(); - _initialized = true; - - warnx("init done."); - continue; - } - - if (_initialized) { - - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; - - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; - - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - - /* run the actual fusion */ - FuseVelposNED(); - } - - } else { - - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; } - } else { - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; + } + + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + baroHgt = _baro.altitude; + + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -689,41 +639,107 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + } - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - FuseMagnetometer(); + + /** + * PART TWO: EXECUTE THE FILTER + **/ + + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + InitialiseFilter(velNED); + _initialized = true; + + warnx("init done."); + } + + if (_initialized) { + + /* predict states and covariances */ + + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(IMUmsec); + + /* evaluate if on ground */ + OnGroundCheck(); + + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; } + } + + + if (gps_updated && _initialized) { + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + + // set flags for further processing + fuseVelData = true; + fusePosData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + } else { + fuseVelData = false; + fusePosData = false; + } + + if (baro_updated && _initialized) { + + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + FuseVelposNED(); + } else { + fuseHgtData = false; + } + + if (mag_updated && _initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); } else { fuseMagData = false; } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - if (airspeed_updated && _initialized) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + if (_initialized) { + FuseMagnetometer(); + } - if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { - - VtasMeas = _airspeed.true_airspeed_m_s; - - if (_initialized) { - - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - } else { - fuseVtasData = false; - } + if (airspeed_updated && _initialized + && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); } else { fuseVtasData = false; } + // Publish results if (_initialized) { // State vector: @@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + printf("dt: %8.6f\n", dtIMU); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); From 0962d041f17beda7d4495c4d450dd39967a11a90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 17:23:03 +0100 Subject: [PATCH 034/134] Estimator init --- src/modules/fw_att_pos_estimator/estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9b57dfd550..f2db4aa1ed 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -322,7 +322,7 @@ void CovariancePrediction() float SF[14]; float SG[8]; float SQ[11]; - float SPP[13]; + float SPP[13] = {0}; float nextP[21][21]; // calculate covariance prediction process noise From 30882e103b8551cbdd40db7c4bc4a8a809893d6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 13:22:58 +0100 Subject: [PATCH 035/134] Moved to using references for arrays --- .../fw_att_pos_estimator/estimator.cpp | 23 +++++++++--------- src/modules/fw_att_pos_estimator/estimator.h | 24 +++++++++---------- 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 95408b729b..24e3f4f162 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -53,7 +53,6 @@ float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; -float gpsGndSpd; float gpsVelD; float gpsLat; float gpsLon; @@ -1560,7 +1559,7 @@ void FuseAirspeed() } } -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1573,7 +1572,7 @@ void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) } } -void zeroCols(float covMat[n_states][n_states], uint8_t first, uint8_t last) +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1604,7 +1603,7 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) { long int timeDelta; long int bestTimeDelta = 200; @@ -1630,7 +1629,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec) } } -void quat2Tnb(Mat3f &Tnb, float quat[4]) +void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix float q00 = sq(quat[0]); @@ -1655,7 +1654,7 @@ void quat2Tnb(Mat3f &Tnb, float quat[4]) Tnb.y.z = 2*(q23 + q01); } -void quat2Tbn(Mat3f &Tbn, float quat[4]) +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1680,7 +1679,7 @@ void quat2Tbn(Mat3f &Tbn, float quat[4]) Tbn.z.y = 2*(q23 + q01); } -void eul2quat(float quat[4], float eul[3]) +void eul2quat(float (&quat)[4], const float (&eul)[3]) { float u1 = cos(0.5f*eul[0]); float u2 = cos(0.5f*eul[1]); @@ -1694,28 +1693,28 @@ void eul2quat(float quat[4], float eul[3]) quat[3] = u1*u2*u6-u4*u5*u3; } -void quat2eul(float y[3], float u[4]) +void quat2eul(float (&y)[3], const float (&u)[4]) { y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2])); y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) { velNED[0] = gpsGndSpd*cos(gpsCourse); velNED[1] = gpsGndSpd*sin(gpsCourse); velNED[2] = gpsVelD; } -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); posNED[2] = -(hgt - hgtRef); } -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); @@ -1799,7 +1798,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float initvelNED[3]) +void InitialiseFilter(float (&initvelNED)[3]) { // Do the data structure init for (unsigned i = 0; i < n_states; i++) { diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 2a0e43bb5e..01a1ace996 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,41 +134,39 @@ void FuseMagnetometer(); void FuseAirspeed(); -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last); +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); -void zeroCols(float covMat[n_states][n_states], uint8_t first, uint8_t last); +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); float sq(float valIn); -void quatNorm(float quatOut[4], float quatIn[4]); +void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec); +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec); -void quat2Tnb(Mat3f &Tnb, float quat[4]); - -void quat2Tbn(Mat3f &Tbn, float quat[4]); +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -void eul2quat(float quat[4], float eul[3]); +void eul2quat(float (&quat)[4], const float (&eul)[3]); -void quat2eul(float eul[3],float quat[4]); +void quat2eul(float (&eul)[3], const float (&quat)[4]); -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float initvelNED[3]); +void InitialiseFilter(float (&initvelNED)[3]); uint32_t millis(); From dc2f40b80d6291d4f9b2060bb1d5c86d704a3ca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 14:23:48 +0100 Subject: [PATCH 036/134] Updated uploader from Bootloader repo --- Tools/px_uploader.py | 48 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce5e5e54a..23dc484505 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -50,6 +50,9 @@ # Currently only used for informational purposes. # +# for python2.7 compatibility +from __future__ import print_function + import sys import argparse import binascii @@ -154,6 +157,8 @@ class uploader(object): PROG_MULTI = b'\x27' READ_MULTI = b'\x28' # rev2 only GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -175,6 +180,8 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' def close(self): if self.port is not None: @@ -237,6 +244,22 @@ class uploader(object): self.__getSync() return value + # send the GET_OTP command and wait for an info parameter + def __getOTP(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_OTP + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + + # send the GET_OTP command and wait for an info parameter + def __getSN(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_SN + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -353,6 +376,31 @@ class uploader(object): if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + print("type: " + self.otp_id.decode('utf-8')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print('') print("erase...") self.__erase() From 274e3c0ba732c7698f64866c37653ccfb72b9eb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 18:30:46 +0100 Subject: [PATCH 037/134] Better status reporting --- .../fw_att_pos_estimator_main.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8efd875575..33f35bfd0d 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -918,6 +918,17 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("states: %s %s %s %s", + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT"); + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS"); + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + exit(0); } else { From 3cad5e352e122f874b5d197388288ec9f8a20c9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 18:34:11 +0100 Subject: [PATCH 038/134] Fixed build error --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 33f35bfd0d..71df13d274 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -918,14 +918,14 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s", + printf("states: %s %s %s %s %s %s %s %s %s", (statesInitialised) ? "INITIALIZED" : "NON_INIT", (onGround) ? "ON_GROUND" : "AIRBORNE", (fuseVelData) ? "FUSE_VEL" : "INH_VEL", (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT"); + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS"); + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); From efecd85658eecd0b0651a21c37d3936589074e91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 23:54:04 +0100 Subject: [PATCH 039/134] Further build cleanup --- src/modules/fw_att_pos_estimator/estimator.cpp | 4 +--- src/modules/fw_att_pos_estimator/estimator.h | 3 +-- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 24e3f4f162..9be12e700f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -22,7 +22,6 @@ Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) -float dt; // time lapsed since last covariance prediction uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -292,7 +291,7 @@ void UpdateStrapdownEquationsNED() } -void CovariancePrediction() +void CovariancePrediction(float dt) { // scalars float windVelSigma; @@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3]) summedDelVel.x = 0.0f; summedDelVel.y = 0.0f; summedDelVel.z = 0.0f; - dt = 0.0f; } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 01a1ace996..088304cb3a 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -68,7 +68,6 @@ extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes cor extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float dt; // time lapsed since last covariance prediction extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) @@ -126,7 +125,7 @@ const float covDelAngMax = 0.05f; // maximum delta angle between covariance pred void UpdateStrapdownEquationsNED(); -void CovariancePrediction(); +void CovariancePrediction(float dt); void FuseVelposNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 71df13d274..23cd985302 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -405,6 +405,8 @@ FixedwingEstimator::task_main() Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + float dt = 0.0f; // time lapsed since last covariance prediction + /* wakeup source(s) */ struct pollfd fds[2]; @@ -675,7 +677,7 @@ FixedwingEstimator::task_main() * or the time limit will be exceeded on the next measurement update */ if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); + CovariancePrediction(dt); summedDelAng = summedDelAng.zero(); summedDelVel = summedDelVel.zero(); dt = 0.0f; From 6b931a2738f4e6100fbd3cb7bdc168489c4f8b85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 08:58:36 +0100 Subject: [PATCH 040/134] Testing: Autostart fake GPS in HIL, to be REMOVED --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6f4e1f3b56..2a3941e594 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -386,6 +386,7 @@ then then sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 + gps start -f usleep 5000 else if [ $TTYS1_BUSY == yes ] From 2fd5c9d277d8c78a19e38e2a44c9d91c6612aed7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:04:51 +0100 Subject: [PATCH 041/134] Removed some debugging, added other, still WIP --- src/modules/fw_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 107 ++++++++++-------- 2 files changed, 60 insertions(+), 49 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 01a1ace996..ce3050cbe6 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,7 +121,7 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions +const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 71df13d274..28e547cbe2 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -99,6 +99,8 @@ uint32_t millis() return IMUmsec; } +static void print_status(); + class FixedwingEstimator { public: @@ -383,11 +385,11 @@ FixedwingEstimator::task_main() /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 6); + orb_set_interval(_gyro_sub, 4); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 6); + orb_set_interval(_sensor_combined_sub, 4); #endif parameters_update(); @@ -502,10 +504,7 @@ FixedwingEstimator::task_main() dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; lastAngRate = angRate; - // dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - dVelIMU.x = 0.0f; - dVelIMU.y = 0.0f; - dVelIMU.z = 0.0f; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; lastAccel = accel; @@ -585,7 +584,7 @@ FixedwingEstimator::task_main() // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; } @@ -616,26 +615,26 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _mag.x; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _mag.y; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _mag.z; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #endif @@ -664,11 +663,13 @@ FixedwingEstimator::task_main() StoreStates(IMUmsec); /* evaluate if on ground */ - OnGroundCheck(); + // OnGroundCheck(); + onGround = false; /* prepare the delta angles and time used by the covariance prediction */ summedDelAng = summedDelAng + correctedDelAng; summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; /* predict the covairance if the total delta angle has exceeded the threshold @@ -680,6 +681,7 @@ FixedwingEstimator::task_main() summedDelVel = summedDelVel.zero(); dt = 0.0f; } + } @@ -730,7 +732,7 @@ FixedwingEstimator::task_main() } if (airspeed_updated && _initialized - && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data @@ -742,6 +744,8 @@ FixedwingEstimator::task_main() // Publish results if (_initialized) { + + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -858,6 +862,45 @@ FixedwingEstimator::start() return OK; } +void print_status() +{ + math::Quaternion q(states[0], states[1], states[2], states[3]); + math::EulerAngles euler(q); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("states: %s %s %s %s %s %s %s %s %s\n", + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); +} + int fw_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) @@ -895,39 +938,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator) { warnx("running"); - math::Quaternion q(states[0], states[1], states[2], states[3]); - math::EulerAngles euler(q); - - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - - printf("dt: %8.6f\n", dtIMU); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s %s %s %s %s %s", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + print_status(); exit(0); From d572424996124b1f2dafdcbd48baf1abc85ee627 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:06:47 +0100 Subject: [PATCH 042/134] Build fix hackery --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9c98a49bbf..b32b3686fd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -357,6 +357,8 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } +static float dt = 0.0f; // time lapsed since last covariance prediction + void FixedwingEstimator::task_main() { @@ -407,8 +409,6 @@ FixedwingEstimator::task_main() Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; - float dt = 0.0f; // time lapsed since last covariance prediction - /* wakeup source(s) */ struct pollfd fds[2]; From c6f6eaf0cd99aadbc93264144cdab8bba81f1937 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Feb 2014 17:15:45 +0100 Subject: [PATCH 043/134] Minor init cleanup --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 294e1fc51d..829782d90e 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -363,10 +363,6 @@ void FixedwingEstimator::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -647,7 +643,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { InitialiseFilter(velNED); _initialized = true; From f6088c2f6ec29abafab07fe2e30aec943d09f46b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Feb 2014 17:38:33 +0100 Subject: [PATCH 044/134] Better initialization, removed unnecessary static variables, reduced scopes where feasible --- .../fw_att_pos_estimator/estimator.cpp | 105 ++++++++++-------- src/modules/fw_att_pos_estimator/estimator.h | 4 +- 2 files changed, 64 insertions(+), 45 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9be12e700f..bae4272340 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,6 +1,7 @@ #include "estimator.h" // For debugging only +#include #include // Global variables @@ -87,6 +88,20 @@ Vector3f Vector3f::zero(void) const return ret; } +Mat3f::Mat3f() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + Mat3f Mat3f::transpose(void) const { Mat3f ret = *this; @@ -165,7 +180,6 @@ void swap_var(float &d1, float &d2) void UpdateStrapdownEquationsNED() { - static Vector3f prevDelAng; Vector3f delVelNav; float q00; float q11; @@ -177,15 +191,12 @@ void UpdateStrapdownEquationsNED() float q12; float q13; float q23; - static Mat3f Tbn; - static Mat3f Tnb; + Mat3f Tbn; + Mat3f Tnb; float rotationMag; - float rotScaler; float qUpdated[4]; float quatMag; - float quatMagInv; float deltaQuat[4]; - static float lastVelocity[3]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; // Remove sensor bias errors @@ -197,7 +208,7 @@ void UpdateStrapdownEquationsNED() dVelIMU.z = dVelIMU.z; // Save current measurements - prevDelAng = correctedDelAng; + Vector3f prevDelAng = correctedDelAng; // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded @@ -215,7 +226,7 @@ void UpdateStrapdownEquationsNED() else { deltaQuat[0] = cos(0.5f*rotationMag); - rotScaler = (sin(0.5f*rotationMag))/rotationMag; + float rotScaler = (sin(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -232,7 +243,7 @@ void UpdateStrapdownEquationsNED() quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { - quatMagInv = 1.0f/quatMag; + float quatMagInv = 1.0f/quatMag; states[0] = quatMagInv*qUpdated[0]; states[1] = quatMagInv*qUpdated[1]; states[2] = quatMagInv*qUpdated[2]; @@ -275,6 +286,7 @@ void UpdateStrapdownEquationsNED() accNavMag = delVelNav.length()/dtIMU; // If calculating position save previous velocity + float lastVelocity[3]; lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; @@ -296,7 +308,7 @@ void CovariancePrediction(float dt) // scalars float windVelSigma; float dAngBiasSigma; - float dVelBiasSigma; + // float dVelBiasSigma; float magEarthSigma; float magBodySigma; float daxCov; @@ -911,9 +923,9 @@ void FuseVelposNED() uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time uint32_t horizRetryTime; - static uint32_t velFailTime; - static uint32_t posFailTime; - static uint32_t hgtFailTime; + static uint32_t velFailTime = 0; + static uint32_t posFailTime = 0; + static uint32_t hgtFailTime = 0; bool velHealth; bool posHealth; bool hgtHealth; @@ -922,9 +934,9 @@ void FuseVelposNED() bool hgtTimeout; // declare variables used to check measurement errors - float velInnov[3] = {0.0,0.0,0.0}; - float posInnov[2] = {0.0,0.0}; - float hgtInnov = 0.0; + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; @@ -1133,32 +1145,19 @@ void FuseVelposNED() void FuseMagnetometer() { - - static float q0 = 1.0; - static float q1 = 0.0; - static float q2 = 0.0; - static float q3 = 0.0; - static float magN = 0.0; - static float magE = 0.0; - static float magD = 0.0; - static float magXbias = 0.0; - static float magYbias = 0.0; - static float magZbias = 0.0; - static uint8_t obsIndex; + uint8_t obsIndex; uint8_t indexLimit; float DCM[3][3] = { - {1.0,0.0,0.0} , - {0.0,1.0,0.0} , - {0.0,0.0,1.0} + {1.0f,0.0f,0.0f} , + {0.0f,1.0f,0.0f} , + {0.0f,0.0f,1.0f} }; - static float MagPred[3] = {0.0,0.0,0.0}; - static float R_MAG; - static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - float H_MAG[21]; + float MagPred[3] = {0.0f,0.0f,0.0f}; float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; + float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1178,12 +1177,28 @@ void FuseMagnetometer() indexLimit = 12; } + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + + static float R_MAG = 0.0025f; + + float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + // Sequential fusion of XYZ components to spread processing load across // three prediction time steps. // Calculate observation jacobians and Kalman gains if (fuseMagData) { + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; q1 = statesAtMagMeasTime[1]; @@ -1224,6 +1239,7 @@ void FuseMagnetometer() SH_MAG[6] = sq(q0); SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; @@ -1232,7 +1248,7 @@ void FuseMagnetometer() H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; H_MAG[16] = 2*q0*q3 + 2*q1*q2; H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1; + H_MAG[18] = 1.0f; // Calculate Kalman gain SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); @@ -1272,7 +1288,7 @@ void FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + for (unsigned int i=0; i 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1592,6 +1607,7 @@ float sq(float valIn) // Store states in a history array along with time stamp void StoreStates(uint64_t timestamp_ms) { + /* static to keep the store index */ static uint8_t storeIndex = 0; for (unsigned i=0; i Date: Sun, 16 Feb 2014 18:21:30 +0100 Subject: [PATCH 045/134] Debug hackery. We finally got something that is kind of close to an actual attitude estimate. --- .../fw_att_pos_estimator_main.cpp | 284 +++++++++++++----- 1 file changed, 211 insertions(+), 73 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 829782d90e..02b58224e1 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -357,7 +357,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } -static float dt = 0.0f; // time lapsed since last covariance prediction +float dt = 0.0f; // time lapsed since last covariance prediction + +// Estimated time delays (msec) +uint32_t msecVelDelay = 230; +uint32_t msecPosDelay = 210; +uint32_t msecHgtDelay = 350; +uint32_t msecMagDelay = 30; +uint32_t msecTasDelay = 210; void FixedwingEstimator::task_main() @@ -403,7 +410,13 @@ FixedwingEstimator::task_main() /* initialize measurement data */ VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; + dVelIMU.x = 0.0f; + dVelIMU.y = 0.0f; + dVelIMU.z = 0.0f; + dAngIMU.x = 0.0f; + dAngIMU.y = 0.0f; + dAngIMU.z = 0.0f; /* wakeup source(s) */ struct pollfd fds[2]; @@ -421,6 +434,10 @@ FixedwingEstimator::task_main() hrt_abstime start_time = hrt_absolute_time(); + bool newDataGps = false; + bool newAdsData = false; + bool newDataMag = false; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -548,6 +565,9 @@ FixedwingEstimator::task_main() if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; + newDataMag = true; + } else { + newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; @@ -560,6 +580,9 @@ FixedwingEstimator::task_main() perf_count(_perf_airspeed); VtasMeas = _airspeed.true_airspeed_m_s; + newAdsData = true; + } else { + newAdsData = false; } bool gps_updated; @@ -570,6 +593,7 @@ FixedwingEstimator::task_main() if (_gps.fix_type < 3) { gps_updated = false; + newDataGps = false; } else { /* fuse GPS updates */ @@ -584,6 +608,7 @@ FixedwingEstimator::task_main() gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; + newDataGps = true; } @@ -636,6 +661,10 @@ FixedwingEstimator::task_main() #endif + newDataMag = true; + + } else { + newDataMag = false; } @@ -643,101 +672,210 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { - InitialiseFilter(velNED); - _initialized = true; + if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) + { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } - warnx("init done."); - } + // If valid IMU data and states initialised, predict states and covariances + if (statesInitialised) + { + // Run the strapdown INS equations every IMU update + UpdateStrapdownEquationsNED(); + #if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; + for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j]; + quat2eul(eulerEst, tempQuat); + for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; + if (eulerDif[2] > pi) eulerDif[2] -= 2*pi; + if (eulerDif[2] < -pi) eulerDif[2] += 2*pi; + #endif + // store the predicted states for subsequent use by measurement fusion + StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + OnGroundCheck(); + onGround = false; + // sum delta angles and time used by covariance prediction + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + dVelIMU; + dt += dtIMU; + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) + { + CovariancePrediction(dt); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } - if (_initialized) { + _initialized = true; + } - /* predict states and covariances */ + // Fuse GPS Measurements + if (newDataGps && statesInitialised) + { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); + } + else + { + fuseVelData = false; + fusePosData = false; + } - /* store the predictions */ - StoreStates(IMUmsec); + if (newAdsData && statesInitialised) + { + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); + // run the fusion step + FuseVelposNED(); + } + else + { + fuseHgtData = false; + } - /* evaluate if on ground */ - // OnGroundCheck(); - onGround = false; + // Fuse Magnetometer Measurements + if (newDataMag && statesInitialised) + { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data + } + else + { + fuseMagData = false; + } + if (statesInitialised) FuseMagnetometer(); - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; + // Fuse Airspeed Measurements + if (newAdsData && statesInitialised && VtasMeas > 8.0f) + { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } + else + { + fuseVtasData = false; + } + + // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { + // InitialiseFilter(velNED); + // _initialized = true; + + // warnx("init done."); + // } + + // if (_initialized) { + + // /* predict states and covariances */ + + // /* run the strapdown INS every sensor update */ + // UpdateStrapdownEquationsNED(); + + // /* store the predictions */ + // StoreStates(IMUmsec); + + // /* evaluate if on ground */ + // // OnGroundCheck(); + // onGround = false; + + // /* prepare the delta angles and time used by the covariance prediction */ + // summedDelAng = summedDelAng + correctedDelAng; + // summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; + // dt += dtIMU; - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } + // /* predict the covairance if the total delta angle has exceeded the threshold + // * or the time limit will be exceeded on the next measurement update + // */ + // if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + // CovariancePrediction(dt); + // summedDelAng = summedDelAng.zero(); + // summedDelVel = summedDelVel.zero(); + // dt = 0.0f; + // } - } + // } - if (gps_updated && _initialized) { + // if (gps_updated && _initialized) { - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + // /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + // calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + // posNE[0] = posNED[0]; + // posNE[1] = posNED[1]; - // set flags for further processing - fuseVelData = true; - fusePosData = true; + // // set flags for further processing + // fuseVelData = true; + // fusePosData = true; - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // /* recall states after adjusting for delays */ + // RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + // RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - /* run the actual fusion */ - FuseVelposNED(); - } else { - fuseVelData = false; - fusePosData = false; - } + // /* run the actual fusion */ + // FuseVelposNED(); + // } else { + // fuseVelData = false; + // fusePosData = false; + // } - if (baro_updated && _initialized) { + // if (baro_updated && _initialized) { - fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - FuseVelposNED(); - } else { - fuseHgtData = false; - } + // fuseHgtData = true; + // // recall states stored at time of measurement after adjusting for delays + // RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // // run the fusion step + // FuseVelposNED(); + // } else { + // fuseHgtData = false; + // } - if (mag_updated && _initialized) { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); + // if (mag_updated && _initialized) { + // fuseMagData = true; + // RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - } else { - fuseMagData = false; - } + // } else { + // fuseMagData = false; + // } - if (_initialized) { - FuseMagnetometer(); - } + // if (_initialized) { + // FuseMagnetometer(); + // } - if (airspeed_updated && _initialized - && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { + // if (airspeed_updated && _initialized + // && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } else { - fuseVtasData = false; - } + // fuseVtasData = true; + // RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + // FuseAirspeed(); + // } else { + // fuseVtasData = false; + // } // Publish results if (_initialized) { From 7e9fcac057616ba535e09d49f7d0f47f5ce9977b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 08:24:30 +0100 Subject: [PATCH 046/134] Pure code style formatting --- .../fw_att_pos_estimator_main.cpp | 278 +++++++++--------- 1 file changed, 142 insertions(+), 136 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 02b58224e1..4f5e3c9b48 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -129,9 +129,9 @@ private: int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ - #else +#else int _sensor_combined_sub; - #endif +#endif int _airspeed_sub; /**< airspeed subscription */ int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ @@ -233,13 +233,13 @@ FixedwingEstimator::FixedwingEstimator() : _estimator_task(-1), /* subscriptions */ - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), - #else +#else _sensor_combined_sub(-1), - #endif +#endif _airspeed_sub(-1), _baro_sub(-1), _gps_sub(-1), @@ -382,7 +382,7 @@ FixedwingEstimator::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); @@ -391,11 +391,11 @@ FixedwingEstimator::task_main() /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 4); - #else +#else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 4); - #endif +#endif parameters_update(); @@ -424,13 +424,13 @@ FixedwingEstimator::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; - #else +#else fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; - #endif +#endif hrt_abstime start_time = hrt_absolute_time(); @@ -483,9 +483,9 @@ FixedwingEstimator::task_main() hrt_abstime last_sensor_timestamp; /* load local copies */ - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - + orb_check(_accel_sub, &accel_updated); @@ -523,7 +523,7 @@ FixedwingEstimator::task_main() lastAccel = accel; - #else +#else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; @@ -532,6 +532,7 @@ FixedwingEstimator::task_main() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } + last_accel = _sensor_combined.accelerometer_timestamp; @@ -566,27 +567,32 @@ FixedwingEstimator::task_main() if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; newDataMag = true; + } else { newDataMag = false; } + last_mag = _sensor_combined.magnetometer_timestamp; - #endif +#endif bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; + } else { newAdsData = false; } bool gps_updated; orb_check(_gps_sub, &gps_updated); + if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -594,6 +600,7 @@ FixedwingEstimator::task_main() if (_gps.fix_type < 3) { gps_updated = false; newDataGps = false; + } else { /* fuse GPS updates */ @@ -616,23 +623,25 @@ FixedwingEstimator::task_main() bool baro_updated; orb_check(_baro_sub, &baro_updated); + if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); baroHgt = _baro.altitude; // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; + hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; } - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); - #endif +#endif + if (mag_updated) { perf_count(_perf_mag); - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. @@ -645,8 +654,8 @@ FixedwingEstimator::task_main() magData.z = _mag.z; magBias.z = 0.000001f; // _mag_offsets.y_offset - - #else + +#else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f @@ -659,7 +668,7 @@ FixedwingEstimator::task_main() magData.z = _sensor_combined.magnetometer_ga[2]; magBias.z = 0.000001f; // _mag_offsets.y_offset - #endif +#endif newDataMag = true; @@ -672,114 +681,111 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) - { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - InitialiseFilter(velNED); - } + if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } - // If valid IMU data and states initialised, predict states and covariances - if (statesInitialised) - { - // Run the strapdown INS equations every IMU update - UpdateStrapdownEquationsNED(); - #if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j]; - quat2eul(eulerEst, tempQuat); - for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - if (eulerDif[2] > pi) eulerDif[2] -= 2*pi; - if (eulerDif[2] < -pi) eulerDif[2] += 2*pi; - #endif - // store the predicted states for subsequent use by measurement fusion - StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - OnGroundCheck(); - onGround = false; - // sum delta angles and time used by covariance prediction - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + dVelIMU; - dt += dtIMU; - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) - { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } + // If valid IMU data and states initialised, predict states and covariances + if (statesInitialised) { + // Run the strapdown INS equations every IMU update + UpdateStrapdownEquationsNED(); +#if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; - _initialized = true; - } + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - // Fuse GPS Measurements - if (newDataGps && statesInitialised) - { - // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + quat2eul(eulerEst, tempQuat); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - // set fusion flags - fuseVelData = true; - fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); - RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); - // run the fusion step - FuseVelposNED(); - } - else - { - fuseVelData = false; - fusePosData = false; - } + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - if (newAdsData && statesInitialised) - { - // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; - fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); - // run the fusion step - FuseVelposNED(); - } - else - { - fuseHgtData = false; - } + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - // Fuse Magnetometer Measurements - if (newDataMag && statesInitialised) - { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data - } - else - { - fuseMagData = false; - } - if (statesInitialised) FuseMagnetometer(); + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - // Fuse Airspeed Measurements - if (newAdsData && statesInitialised && VtasMeas > 8.0f) - { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - else - { - fuseVtasData = false; - } +#endif + // store the predicted states for subsequent use by measurement fusion + StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + OnGroundCheck(); + onGround = false; + // sum delta angles and time used by covariance prediction + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + dVelIMU; + dt += dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(dt); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } + + _initialized = true; + } + + // Fuse GPS Measurements + if (newDataGps && statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); + + } else { + fuseVelData = false; + fusePosData = false; + } + + if (newAdsData && statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); + // run the fusion step + FuseVelposNED(); + + } else { + fuseHgtData = false; + } + + // Fuse Magnetometer Measurements + if (newDataMag && statesInitialised) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data + + } else { + fuseMagData = false; + } + + if (statesInitialised) FuseMagnetometer(); + + // Fuse Airspeed Measurements + if (newAdsData && statesInitialised && VtasMeas > 8.0f) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + + } else { + fuseVtasData = false; + } // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { // InitialiseFilter(velNED); @@ -805,7 +811,7 @@ FixedwingEstimator::task_main() // /* prepare the delta angles and time used by the covariance prediction */ // summedDelAng = summedDelAng + correctedDelAng; // summedDelVel = summedDelVel + correctedDelVel; - + // dt += dtIMU; // /* predict the covairance if the total delta angle has exceeded the threshold @@ -829,7 +835,7 @@ FixedwingEstimator::task_main() // posNE[0] = posNED[0]; // posNE[1] = posNED[1]; - // // set flags for further processing + // // set flags for further processing // fuseVelData = true; // fusePosData = true; @@ -896,7 +902,7 @@ FixedwingEstimator::task_main() math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); + _att.R[i][j] = R(i, j); _att.timestamp = last_sensor_timestamp; _att.q[0] = states[0]; @@ -1005,7 +1011,7 @@ void print_status() math::Vector<3> euler = R.to_euler(); printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) @@ -1027,15 +1033,15 @@ void print_status() printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); printf("states: %s %s %s %s %s %s %s %s %s\n", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); } int fw_att_pos_estimator_main(int argc, char *argv[]) From 447d159964e3af6c4ac2004d3a9e3217c0f269b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 08:33:58 +0100 Subject: [PATCH 047/134] Initialize the filter immediately, re-init once GPS becomes available (needs in-flight testing) --- .../fw_att_pos_estimator_main.cpp | 46 ++++++++++++++++--- 1 file changed, 40 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 4f5e3c9b48..2b7cdc1189 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -681,11 +681,26 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - InitialiseFilter(velNED); + // Wait long enough to ensure all sensors updated once + // XXX we rather want to check all updated + + + if (hrt_elapsed_time(&start_time) > 100000) { + + if (!_gps_initialized && (GPSstatus == 3)) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + + _gps_initialized = true; + + } else if (!statesInitialised) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } } // If valid IMU data and states initialised, predict states and covariances @@ -730,7 +745,7 @@ FixedwingEstimator::task_main() } // Fuse GPS Measurements - if (newDataGps && statesInitialised) { + if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; @@ -748,6 +763,25 @@ FixedwingEstimator::task_main() // run the fusion step FuseVelposNED(); + } else if (statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); } else { fuseVelData = false; fusePosData = false; From c5311b18fef0e215c019a4686ca9697224d1cd31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 13:22:04 +0100 Subject: [PATCH 048/134] Build fixes --- src/modules/fw_att_pos_estimator/estimator.cpp | 6 ------ .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index bae4272340..0f2559c58b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,9 +1,5 @@ #include "estimator.h" -// For debugging only -#include -#include - // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -1840,8 +1836,6 @@ void InitialiseFilter(float (&initvelNED)[3]) Vector3f initMagXYZ; initMagXYZ = magData - magBias; AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); - printf("initializing: accel: %8.4f %8.4f %8.4f, mag: %8.4f %8.4f %8.4f q: %8.4f %8.4f %8.4f %8.4f\n", - accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat[0], initQuat[1], initQuat[2], initQuat[3]); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 2b7cdc1189..688a44c470 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -172,6 +172,7 @@ private: perf_counter_t _perf_airspeed; /// Date: Fri, 21 Feb 2014 13:44:34 +0100 Subject: [PATCH 049/134] GPS reinit completed, put in NaN catcher --- src/modules/fw_att_pos_estimator/estimator.h | 4 ++-- .../fw_att_pos_estimator_main.cpp | 20 ++++++++++++++++--- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index bc71810183..ff31a84acc 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -122,8 +122,8 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions +const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions +const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 688a44c470..95549ad219 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -694,13 +694,20 @@ FixedwingEstimator::task_main() velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; InitialiseFilter(velNED); + warnx("GPS REINIT"); _gps_initialized = true; } else if (!statesInitialised) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; InitialiseFilter(velNED); } } @@ -970,7 +977,14 @@ FixedwingEstimator::task_main() /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + } + if (!isfinite(states[0])) { + print_status(); + _exit(0); + } + + if (_gps_initialized) { _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; From b36bf7b17e4827465d1d543a00ea3898813c77ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 15:59:56 -0800 Subject: [PATCH 050/134] Removed gravity and on ground check hardcoded testing values --- src/modules/fw_att_pos_estimator/estimator.h | 2 +- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index ff31a84acc..d65be322f7 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -3,7 +3,7 @@ #pragma once -#define GRAVITY_MSS 9.76f//9.80665f +#define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f #define pi 3.141592657f diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 95549ad219..38162959d3 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -735,7 +735,6 @@ FixedwingEstimator::task_main() StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction OnGroundCheck(); - onGround = false; // sum delta angles and time used by covariance prediction summedDelAng = summedDelAng + correctedDelAng; summedDelVel = summedDelVel + dVelIMU; From acb1bc1383399465f3183df6a61698dbc9f5e958 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 18:31:44 -0800 Subject: [PATCH 051/134] Disable time compensation which gets us reasonable results --- src/modules/fw_att_pos_estimator/estimator.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 0f2559c58b..a7c2cb0e35 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1629,14 +1629,14 @@ void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) bestTimeDelta = timeDelta; } } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - } - else // otherwise output current state - { + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - } + // } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) From b4c1713b96d452efa8c2285537f0969d1e6dcb77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 18:55:06 -0800 Subject: [PATCH 052/134] Fix up time delay compensation loading --- .../fw_att_pos_estimator/estimator.cpp | 30 +++++++++---------- src/modules/fw_att_pos_estimator/estimator.h | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index a7c2cb0e35..e449a66028 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -6,8 +6,6 @@ float KHP[n_states][n_states]; // intermediate result used for covariance update float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix -float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps -uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) @@ -55,6 +53,9 @@ float gpsLon; float gpsHgt; uint8_t GPSstatus; +float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps +uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + // Baro input float baroHgt; @@ -1614,14 +1615,13 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { long int bestTimeDelta = 200; - uint8_t storeIndex; - uint8_t bestStoreIndex = 0; - for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++) + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - long int timeDelta = msec - statetimeStamp[storeIndex]; + int64_t timeDelta = (int64_t)msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { @@ -1629,14 +1629,14 @@ void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) bestTimeDelta = timeDelta; } } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } - // else // otherwise output current state - // { + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + } + else // otherwise output current state + { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - // } + } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) @@ -1825,7 +1825,7 @@ void InitialiseFilter(float (&initvelNED)[3]) for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { - + storedStates[j][i] = 0.0f; } statetimeStamp[i] = 0; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d65be322f7..d68ac1d34e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint32_t msec); +void RecallStates(float (&statesForFusion)[n_states], uint64_t msec); void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); From 76ac14d3c4e44c22b04b312acfa34186dfb028d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 19:05:32 -0800 Subject: [PATCH 053/134] Use right combination of casts --- src/modules/fw_att_pos_estimator/estimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e449a66028..c9e809280d 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1617,11 +1617,11 @@ void StoreStates(uint64_t timestamp_ms) // Output the state vector stored at the time that best matches that specified by msec void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { - long int bestTimeDelta = 200; + int64_t bestTimeDelta = 200; unsigned bestStoreIndex = 0; for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - int64_t timeDelta = (int64_t)msec - statetimeStamp[storeIndex]; + int64_t timeDelta = (int)msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { From dac0427b602be5de9058e78851a9182fd236ce0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Feb 2014 17:17:47 -0800 Subject: [PATCH 054/134] fw_att_pos_estimator: Removed unused code --- .../fw_att_pos_estimator_main.cpp | 97 +------------------ 1 file changed, 1 insertion(+), 96 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 38162959d3..bb3541a396 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -790,6 +790,7 @@ FixedwingEstimator::task_main() RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); // run the fusion step FuseVelposNED(); + } else { fuseVelData = false; fusePosData = false; @@ -829,102 +830,6 @@ FixedwingEstimator::task_main() fuseVtasData = false; } - // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { - // InitialiseFilter(velNED); - // _initialized = true; - - // warnx("init done."); - // } - - // if (_initialized) { - - // /* predict states and covariances */ - - // /* run the strapdown INS every sensor update */ - // UpdateStrapdownEquationsNED(); - - // /* store the predictions */ - // StoreStates(IMUmsec); - - // /* evaluate if on ground */ - // // OnGroundCheck(); - // onGround = false; - - // /* prepare the delta angles and time used by the covariance prediction */ - // summedDelAng = summedDelAng + correctedDelAng; - // summedDelVel = summedDelVel + correctedDelVel; - - // dt += dtIMU; - - // /* predict the covairance if the total delta angle has exceeded the threshold - // * or the time limit will be exceeded on the next measurement update - // */ - // if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - // CovariancePrediction(dt); - // summedDelAng = summedDelAng.zero(); - // summedDelVel = summedDelVel.zero(); - // dt = 0.0f; - // } - - // } - - - // if (gps_updated && _initialized) { - - // /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - // calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - // posNE[0] = posNED[0]; - // posNE[1] = posNED[1]; - - // // set flags for further processing - // fuseVelData = true; - // fusePosData = true; - - // /* recall states after adjusting for delays */ - // RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - // RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - - // /* run the actual fusion */ - // FuseVelposNED(); - // } else { - // fuseVelData = false; - // fusePosData = false; - // } - - // if (baro_updated && _initialized) { - - // fuseHgtData = true; - // // recall states stored at time of measurement after adjusting for delays - // RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // // run the fusion step - // FuseVelposNED(); - // } else { - // fuseHgtData = false; - // } - - // if (mag_updated && _initialized) { - // fuseMagData = true; - // RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - - // } else { - // fuseMagData = false; - // } - - // if (_initialized) { - // FuseMagnetometer(); - // } - - // if (airspeed_updated && _initialized - // && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { - - // fuseVtasData = true; - // RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - // FuseAirspeed(); - // } else { - // fuseVtasData = false; - // } - // Publish results if (_initialized) { From d71396e556a4a729d7fdf14244c1592477cb399e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:16:57 -0800 Subject: [PATCH 055/134] Move params to MAVLink params, enable mavlink text feedback. --- .../fw_att_pos_estimator_main.cpp | 52 ++++++++++--------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index bb3541a396..7e179d400f 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include "estimator.h" @@ -174,18 +175,22 @@ private: bool _initialized; bool _gps_initialized; + int _mavlink_fd; + struct { - float throttle_cruise; - uint32_t vel_delay_ms; - uint32_t pos_delay_ms; - uint32_t height_delay_ms; - uint32_t mag_delay_ms; - uint32_t tas_delay_ms; + int32_t vel_delay_ms; + int32_t pos_delay_ms; + int32_t height_delay_ms; + int32_t mag_delay_ms; + int32_t tas_delay_ms; } _parameters; /**< local copies of interesting parameters */ struct { - param_t throttle_cruise; - + param_t vel_delay_ms; + param_t pos_delay_ms; + param_t height_delay_ms; + param_t mag_delay_ms; + param_t tas_delay_ms; } _parameter_handles; /**< handles for interesting parameters */ @@ -264,10 +269,17 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _initialized(false), - _gps_initialized(false) + _gps_initialized(false), + _mavlink_fd(-1) { - _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); + _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); + _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); + _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); + _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); /* fetch initial parameter values */ parameters_update(); @@ -327,14 +339,11 @@ int FixedwingEstimator::parameters_update() { - // XXX NEED TO GET HANDLES FIRST! NEEDS PARAM INIT - //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); - - _parameters.vel_delay_ms = 230; - _parameters.pos_delay_ms = 210; - _parameters.height_delay_ms = 350; - _parameters.mag_delay_ms = 30; - _parameters.tas_delay_ms = 210; + param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); + param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); + param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); + param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); + param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); return OK; } @@ -361,13 +370,6 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) float dt = 0.0f; // time lapsed since last covariance prediction -// Estimated time delays (msec) -uint32_t msecVelDelay = 230; -uint32_t msecPosDelay = 210; -uint32_t msecHgtDelay = 350; -uint32_t msecMagDelay = 30; -uint32_t msecTasDelay = 210; - void FixedwingEstimator::task_main() { From 2dd5636ee0763d261ad40330a42dd9a271accb9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:17:31 -0800 Subject: [PATCH 056/134] Add and enable all filter params. --- .../fw_att_pos_estimator_params.c | 67 ++++++++++++++++++- 1 file changed, 66 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c index 64e593a15f..6138ef39cd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -48,5 +48,70 @@ * */ -PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); +/** + * Velocity estimate delay + * + * The delay in milliseconds of the velocity estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230); + +/** + * Position estimate delay + * + * The delay in milliseconds of the position estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210); + +/** + * Height estimate delay + * + * The delay in milliseconds of the height estimate from the barometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350); + +/** + * Mag estimate delay + * + * The delay in milliseconds of the magnetic field estimate from + * the magnetometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30); + +/** + * True airspeeed estimate delay + * + * The delay in milliseconds of the airspeed estimate. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); + +/** + * GPS vs. barometric altitude update weight + * + * RE-CHECK this. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); From 44c5726703160619b2a49f4eb4c3d0b0d07925fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:20:36 -0800 Subject: [PATCH 057/134] Make baro altitude relative. --- .../fw_att_pos_estimator_main.cpp | 97 +++++++++++++++---- 1 file changed, 77 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7e179d400f..c8ef008d93 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -164,6 +164,8 @@ private: struct sensor_combined_s _sensor_combined; #endif + float _baro_ref; /**< barometer reference altitude */ + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// 8.0f) { fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); } else { @@ -900,10 +920,13 @@ FixedwingEstimator::task_main() _local_pos.vy = states[5]; _local_pos.vz = states[6]; - _local_pos.xy_valid = true; + _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; - _local_pos.v_xy_valid = true; + _local_pos.v_xy_valid = _gps_initialized; _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _local_pos.z_global = false; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { @@ -915,17 +938,51 @@ FixedwingEstimator::task_main() _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } - _global_pos.timestamp = _gyro.timestamp; + _global_pos.timestamp = _local_pos.timestamp; - // /* lazily publish the global position only once available */ - // if (_global_pos_pub > 0) { - // /* publish the attitude setpoint */ - // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + _global_pos.baro_valid = true; + _global_pos.global_valid = true; - // } else { - // /* advertise and publish */ - // _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); - // } + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + } + + /* set valid values even if position is not valid */ + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + _global_pos.alt = _local_pos.ref_alt - _local_pos.z; + + if (_local_pos.z_valid) { + _global_pos.baro_alt = _baro_ref - _local_pos.z; + } + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } } } From d64fa6e255450b3b642acf6b8bf29e86ed87753d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:29:43 -0800 Subject: [PATCH 058/134] Handling GPS and baro offset in altitude init / estimate --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8ef008d93..d598052222 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -164,7 +164,8 @@ private: struct sensor_combined_s _sensor_combined; #endif - float _baro_ref; /**< barometer reference altitude */ + float _baro_ref; /**< barometer reference altitude */ + float _baro_gps_offset; /**< offset between GPS and baro */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// Date: Mon, 3 Mar 2014 14:23:49 +0100 Subject: [PATCH 059/134] Fixes to altitude units --- .../fw_att_pos_estimator_main.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index d598052222..082c695dc9 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -701,23 +701,24 @@ FixedwingEstimator::task_main() velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; + + double lat = _gps.lat * 1e-7; + double lon = _gps.lon * 1e-7; + float alt = _gps.alt * 1e-3; + InitialiseFilter(velNED); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.lon; - _local_pos.ref_alt = _gps.alt; + _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; // Store _baro_ref = baroHgt; - _baro_gps_offset = baroHgt - _gps.alt; + _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; - map_projection_init(lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); From b809fad3b6d1e14ad7fb32174136e1c2c21f1da2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Mar 2014 14:24:17 +0100 Subject: [PATCH 060/134] Fixes to startup scripts, back to nominal defaults --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 36194ad682..d114fe21a2 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." +echo "X Plane HIL starting.." set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b471a2a5c4..2356dba541 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -385,7 +385,7 @@ then then sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 - gps start -f + gps start usleep 5000 else if [ $TTYS1_BUSY == yes ] From 40deb405c1e9232475c4b5e7bda8080f85fec19a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Mar 2014 08:35:14 +0100 Subject: [PATCH 061/134] Modified estimator to fix internal GCC compiler error (hilarious 64bit handling), use float instead of double routines to avoid implicit casts --- src/modules/fw_att_pos_estimator/estimator.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c9e809280d..cbbb5d7a0c 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1617,12 +1617,18 @@ void StoreStates(uint64_t timestamp_ms) // Output the state vector stored at the time that best matches that specified by msec void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { - int64_t bestTimeDelta = 200; + int bestTimeDelta = 200; unsigned bestStoreIndex = 0; for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - int64_t timeDelta = (int)msec - statetimeStamp[storeIndex]; - if (timeDelta < 0) timeDelta = -timeDelta; + // The time delta can also end up as negative number, + // since we might compare future to past or past to future + // therefore cast to int64. + int timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + if (timeDelta < 0) { + timeDelta = -timeDelta; + } + if (timeDelta < bestTimeDelta) { bestStoreIndex = storeIndex; @@ -1739,9 +1745,9 @@ void OnGroundCheck() void calcEarthRateNED(Vector3f &omega, float latitude) { //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cos(latitude); - omega.y = 0.0; - omega.z = -earthRate*sin(latitude); + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); } void CovarianceInit() From ebab4bfa7ef9ca138ac476943a87b8e3244b9a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 13:16:06 +0100 Subject: [PATCH 062/134] Fix for recallstates function --- .../fw_att_pos_estimator/estimator.cpp | 50 +++++++++---------- src/modules/fw_att_pos_estimator/estimator.h | 2 +- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index cbbb5d7a0c..f578901465 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1615,34 +1615,34 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) +void RecallStates(float statesForFusion[n_states], uint64_t msec) { - int bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // The time delta can also end up as negative number, - // since we might compare future to past or past to future - // therefore cast to int64. - int timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - if (timeDelta < 0) { - timeDelta = -timeDelta; - } + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - } - else // otherwise output current state - { + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - } + // } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d68ac1d34e..459515f3b8 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec); +void RecallStates(float statesForFusion[n_states], uint64_t msec); void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); From 0581c7f712355a6726e84b302667ddc8cc9f6fa7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: [PATCH 063/134] Make PX4IO driver obey HIL as it should --- src/drivers/px4io/px4io.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8e990aa6f0..82f3ba044a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1959,8 +1962,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; From f7bd31b6fd949cf9bac11cdcda0748138d623be8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: [PATCH 064/134] Teach RGB led driver to forward unknown IOCTLs --- src/drivers/rgbled/rgbled.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed3..e390140de3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); break; } From 4038d3e04eb28295b020ca935d6e4833611aa7e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 15:28:08 +0100 Subject: [PATCH 065/134] Write out yaw into position topics --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 082c695dc9..def437b351 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -931,6 +931,7 @@ FixedwingEstimator::task_main() _local_pos.xy_global = true; _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { From fd8cae19bdb4ba5e97c810f266f78af1461b51b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:53:30 +0100 Subject: [PATCH 066/134] ADC channel mapping fix in sensors app --- src/modules/sensors/sensors.cpp | 11 +++++++---- src/modules/uORB/topics/sensor_combined.h | 3 ++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..6560dec3d6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,16 +1169,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of eight channels */ - struct adc_msg_s buf_adc[8]; + /* make space for a maximum of ten channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[10]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; } /* look for specific channels and process the raw voltage to measurement data */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..918389f38b 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,7 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[9]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ From 4e6e40e5d850f737b25d657630ac89ad1d54e67c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:54:39 +0100 Subject: [PATCH 067/134] Fix up ADC tests command to read all channels at once --- src/systemcmds/tests/test_adc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 030ac6e23c..7302f63cce 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of eight channels */ - struct adc_msg_s data[8]; + /* make space for a maximum of ten channels */ + struct adc_msg_s data[10]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); From d940bdf7f64268d46503cf4b0b996bec3036603e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 17:02:10 +0100 Subject: [PATCH 068/134] Consistently making space for a maximum of 12 channels, 10 channels (including reference / temperature) are used / available right now --- src/drivers/stm32/adc/adc.cpp | 2 +- src/modules/sensors/sensors.cpp | 4 ++-- src/modules/uORB/topics/sensor_combined.h | 4 ++-- src/systemcmds/tests/test_adc.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b82..0b8a275e6a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6560dec3d6..8da5bb2946 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,8 +1169,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of ten channels (to ensure reading all channels at once) */ - struct adc_msg_s buf_adc[10]; + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 918389f38b..1171054707 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[9]; /**< Channel indices of each of these values */ + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7302f63cce..03391b8514 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of ten channels */ - struct adc_msg_s data[10]; + /* make space for a maximum of twelve channels */ + struct adc_msg_s data[12]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); From c3010e5607b6764db6e9eb1a01463903e95acd4e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 18:42:03 +0100 Subject: [PATCH 069/134] Fixed comment that lied --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index def437b351..e0c2215b6b 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1037,8 +1037,8 @@ void print_status() // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); From 37513eaafa80d030ca6f8157f10404608f40fb8a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 18:44:37 +0100 Subject: [PATCH 070/134] Added variance and state contrain calls. Need still in-flight re-init and sub-component health checks. Also need to report / log these events as they occur with enough data to identify root causes. --- .../fw_att_pos_estimator/estimator.cpp | 104 ++++++++++++++++++ src/modules/fw_att_pos_estimator/estimator.h | 6 + 2 files changed, 110 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f578901465..10e9298ed1 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1776,7 +1776,111 @@ void CovarianceInit() P[20][20] = P[18][18]; } +float ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} +void ConstrainVariances() +{ + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + // Constrain quaternion variances + for (unsigned i = 0; i < 4; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocitie variances + for (unsigned i = 4; i < 7; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i < 10; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Angle bias variances + for (unsigned i = 10; i < 13; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + } + + // Wind velocity variances + for (unsigned i = 13; i < 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 15; i < 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 18; i < 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + +} + +void ConstrainStates() +{ + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + + // Constrain quaternion + for (unsigned i = 0; i < 4; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i < 7; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i < 9; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i < 13; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 13; i < 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 15; i < 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 18; i < 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + +} void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 459515f3b8..43bd697934 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -169,5 +169,11 @@ void CovarianceInit(); void InitialiseFilter(float (&initvelNED)[3]); +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + uint32_t millis(); From 03ccee289ba6484a889ed6f79fd744b412bc8537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:19:57 +0100 Subject: [PATCH 071/134] Numerical checks on covariances --- .../fw_att_pos_estimator/estimator.cpp | 192 ++++++++++++++++-- src/modules/fw_att_pos_estimator/estimator.h | 11 + .../fw_att_pos_estimator_main.cpp | 5 +- 3 files changed, 184 insertions(+), 24 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 10e9298ed1..e21b94c43f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,7 @@ #include "estimator.h" +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -67,9 +69,21 @@ bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused bool fuseMagData = false; // boolean true when magnetometer data is to be fused bool fuseVtasData = false; // boolean true when airspeed data is to be fused -bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) -bool useAirspeed = true; // boolean true if airspeed data is being used -bool useCompass = true; // boolean true if magnetometer data is being used +bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) +bool staticMode = true; ///< boolean true if no position feedback is fused +bool useAirspeed = true; ///< boolean true if airspeed data is being used +bool useCompass = true; ///< boolean true if magnetometer data is being used + +bool velHealth; +bool posHealth; +bool hgtHealth; +bool velTimeout; +bool posTimeout; +bool hgtTimeout; + +bool numericalProtection = true; + +static unsigned storeIndex = 0; float Vector3f::length(void) const { @@ -889,7 +903,7 @@ void CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (uint8_t j=0; j<=20; j++) + for (unsigned j = 0; j < n_states; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -897,19 +911,39 @@ void CovariancePrediction(float dt) } } - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i<=20; i++) - { - for (uint8_t j=0; j<=i-1; j++) - { - P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + ForceSymmetry(); } - // + ConstrainVariances(); } void FuseVelposNED() @@ -923,12 +957,6 @@ void FuseVelposNED() static uint32_t velFailTime = 0; static uint32_t posFailTime = 0; static uint32_t hgtFailTime = 0; - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1137,6 +1165,9 @@ void FuseVelposNED() } } + ForceSymmetry(); + ConstrainVariances(); + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } @@ -1442,6 +1473,9 @@ void FuseMagnetometer() } } obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); } void FuseAirspeed() @@ -1568,6 +1602,9 @@ void FuseAirspeed() } } } + + ForceSymmetry(); + ConstrainVariances(); } void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -1604,8 +1641,6 @@ float sq(float valIn) // Store states in a history array along with time stamp void StoreStates(uint64_t timestamp_ms) { - /* static to keep the store index */ - static uint8_t storeIndex = 0; for (unsigned i=0; i GPS_FIX_2D); + } } void calcEarthRateNED(Vector3f &omega, float latitude) @@ -1783,6 +1841,10 @@ float ConstrainFloat(float val, float min, float max) void ConstrainVariances() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1831,6 +1893,10 @@ void ConstrainVariances() void ConstrainStates() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1882,6 +1948,88 @@ void ConstrainStates() } +void ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (posTimeout || velTimeout || hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 43bd697934..b8b218e396 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,10 +121,19 @@ extern uint8_t GPSstatus; extern float baroHgt; extern bool statesInitialised; +extern bool numericalProtection; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions +extern bool staticMode; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -175,5 +184,7 @@ void ConstrainVariances(); void ConstrainStates(); +void ForceSymmetry(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e0c2215b6b..df2608f831 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1050,7 +1050,7 @@ void print_status() printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s %s %s %s %s %s\n", + printf("states: %s %s %s %s %s %s %s %s %s %s\n", (statesInitialised) ? "INITIALIZED" : "NON_INIT", (onGround) ? "ON_GROUND" : "AIRBORNE", (fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1059,7 +1059,8 @@ void print_status() (fuseMagData) ? "FUSE_MAG" : "INH_MAG", (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } int fw_att_pos_estimator_main(int argc, char *argv[]) From 7cdb7291af52f2b60aa53607e7ec229fecf7497f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:21:27 +0100 Subject: [PATCH 072/134] Protect against divergence --- src/modules/fw_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e21b94c43f..fa81d4dfa0 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -312,6 +312,8 @@ void UpdateStrapdownEquationsNED() states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + // Constrain states (to protect against filter divergence) + ConstrainStates(); } void CovariancePrediction(float dt) From bce67c6b036cddbe1542f910c6e605256283768f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:28:54 +0100 Subject: [PATCH 073/134] Init / reinit improvements --- .../fw_att_pos_estimator/estimator.cpp | 41 ++++++++++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 8 ++++ .../fw_att_pos_estimator_main.cpp | 21 +++++++++- 3 files changed, 68 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index fa81d4dfa0..36b9f4491b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1651,7 +1651,7 @@ void StoreStates(uint64_t timestamp_ms) storeIndex = 0; } -void ResetStates() +void ResetStoredStates() { // reset all stored states memset(&storedStates[0][0], 0, sizeof(storedStates)); @@ -2032,6 +2032,45 @@ void ResetVelocity(void) } } + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +void CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index b8b218e396..256aff7716 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -158,6 +158,8 @@ void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec void RecallStates(float statesForFusion[n_states], uint64_t msec); +void ResetStoredStates(); + void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); @@ -186,5 +188,11 @@ void ConstrainStates(); void ForceSymmetry(); +void CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index df2608f831..8b41e7479d 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -604,6 +604,9 @@ FixedwingEstimator::task_main() orb_check(_gps_sub, &gps_updated); if (gps_updated) { + + uint64_t last_gps = _gps.timestamp_position; + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -612,6 +615,14 @@ FixedwingEstimator::task_main() newDataGps = false; } else { + + /* check if we had a GPS outage for a long time */ + if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { + ResetPosition(); + ResetVelocity(); + ResetStoredStates(); + } + /* fuse GPS updates */ //_gps.timestamp / 1e3; @@ -687,6 +698,12 @@ FixedwingEstimator::task_main() } + /** + * CHECK IF THE INPUT DATA IS SANE + */ + CheckAndBound(); + + /** * PART TWO: EXECUTE THE FILTER **/ @@ -715,7 +732,9 @@ FixedwingEstimator::task_main() _local_pos.ref_timestamp = _gps.timestamp_position; // Store - _baro_ref = baroHgt; + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref = _baro.altitude; + baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe From ab088c8ac65bfb220ac5ed0853b243026e77600a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:06:06 +0100 Subject: [PATCH 074/134] Altitute wip --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8b41e7479d..e46dffde3c 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -735,7 +735,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref = _baro.altitude; baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = baroHgt - alt; + _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe map_projection_init(lat, lon); @@ -984,10 +984,11 @@ FixedwingEstimator::task_main() _global_pos.vel_e = 0.0f; } - _global_pos.alt = _local_pos.ref_alt - _local_pos.z; + /* local pos alt is negative, change sign and add alt offset */ + _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); if (_local_pos.z_valid) { - _global_pos.baro_alt = _baro_ref - _baro_gps_offset - _local_pos.z; + _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; } if (_local_pos.v_z_valid) { From 42bba678939a58e59282d4c63dbdef53f962e0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:07 +0100 Subject: [PATCH 075/134] Move changes regarding the filtered airspeed consistently across sensors, use actual air temperature instead of board temperature --- src/drivers/ets_airspeed/ets_airspeed.cpp | 5 ++++- src/modules/sensors/sensors.cpp | 6 +++++- src/modules/uORB/topics/differential_pressure.h | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d27ab9727a..d873a1132e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -176,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = (float)diff_pres_pa; + + // XXX we may want to smooth out the readings to remove noise. + report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91a8d5670f..13afe27c8b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,10 +1031,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + raw.baro_pres_mbar * 1e2f, air_temperature_celcius); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1239,6 +1241,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.temperature = -1000.0f; _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 3592c023c5..ff88b04c6e 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,7 @@ struct differential_pressure_s { float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 5113b4a365216987ca2f95b986538f3d3de8734f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:46 +0100 Subject: [PATCH 076/134] mavlink: Hotfixed HIL battery status publication --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 489d2bdcb7..d7e300670a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,13 +634,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } From 5693a9d3b6e923b2a2c05594f10cb5366ee6c495 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 18:28:13 +0100 Subject: [PATCH 077/134] new fixed wing estimator: Fix the symmetry force step of the covariance prediction. --- src/modules/fw_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 36b9f4491b..e6f1fee874 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -876,7 +876,7 @@ void CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; - for (uint8_t i=0; i<= 20; i++) + for (unsigned i = 0; i < n_states; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -942,7 +942,16 @@ void CovariancePrediction(float dt) P[i][i] = nextP[i][i]; } - ForceSymmetry(); + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + } ConstrainVariances(); From 01f33df70718b7a7dba342fb5920d91b3cd83c09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:04:35 +0100 Subject: [PATCH 078/134] Added EKF filter health status reporting, added dynamic in-air reset. --- .../fw_att_pos_estimator/estimator.cpp | 209 +++++++++++++----- src/modules/fw_att_pos_estimator/estimator.h | 29 ++- .../fw_att_pos_estimator_main.cpp | 65 +++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/estimator_status.h | 81 +++++++ 5 files changed, 328 insertions(+), 59 deletions(-) create mode 100644 src/modules/uORB/topics/estimator_status.h diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e6f1fee874..bde46ba036 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -74,12 +74,8 @@ bool staticMode = true; ///< boolean true if no position feedback is fused bool useAirspeed = true; ///< boolean true if airspeed data is being used bool useCompass = true; ///< boolean true if magnetometer data is being used -bool velHealth; -bool posHealth; -bool hgtHealth; -bool velTimeout; -bool posTimeout; -bool hgtTimeout; +struct ekf_status_report current_ekf_state; +struct ekf_status_report last_ekf_error; bool numericalProtection = true; @@ -965,9 +961,6 @@ void FuseVelposNED() uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time uint32_t horizRetryTime; - static uint32_t velFailTime = 0; - static uint32_t posFailTime = 0; - static uint32_t hgtFailTime = 0; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1033,16 +1026,16 @@ void FuseVelposNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; } // apply a 5-sigma threshold - velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - velTimeout = (millis() - velFailTime) > horizRetryTime; - if (velHealth || velTimeout) + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) { - velHealth = true; - velFailTime = millis(); + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); } else { - velHealth = false; + current_ekf_state.velHealth = false; } } if (fusePosData) @@ -1053,16 +1046,16 @@ void FuseVelposNED() varInnovVelPos[3] = P[7][7] + R_OBS[3]; varInnovVelPos[4] = P[8][8] + R_OBS[4]; // apply a 10-sigma threshold - posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - posTimeout = (millis() - posFailTime) > horizRetryTime; - if (posHealth || posTimeout) + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) { - posHealth = true; - posFailTime = millis(); + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); } else { - posHealth = false; + current_ekf_state.posHealth = false; } } // test height measurements @@ -1071,37 +1064,37 @@ void FuseVelposNED() hgtInnov = statesAtHgtTime[9] + hgtMea; varInnovVelPos[5] = P[9][9] + R_OBS[5]; // apply a 10-sigma threshold - hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime; - if (hgtHealth || hgtTimeout) + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) { - hgtHealth = true; - hgtFailTime = millis(); + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); } else { - hgtHealth = false; + current_ekf_state.hgtHealth = false; } } // Set range for sequential fusion of velocity and position measurements depending // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && velHealth) + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; fuseData[2] = true; } - if (fuseVelData && fusionModeGPS == 1 && velHealth) + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; } - if (fusePosData && fusionModeGPS <= 2 && posHealth) + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) { fuseData[3] = true; fuseData[4] = true; } - if (fuseHgtData && hgtHealth) + if (fuseHgtData && current_ekf_state.hgtHealth) { fuseData[5] = true; } @@ -1986,7 +1979,7 @@ bool FilterHealthy() // XXX Check state vector for NaNs and ill-conditioning // Check if any of the major inputs timed out - if (posTimeout || velTimeout || hgtTimeout) { + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { return false; } @@ -2042,6 +2035,65 @@ void ResetVelocity(void) } +void FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + /** * Check the filter inputs and bound its operational state * @@ -2051,21 +2103,30 @@ void ResetVelocity(void) * updated, but before any of the fusion steps are * executed. */ -void CheckAndBound() +int CheckAndBound() { // Store the old filter state bool currStaticMode = staticMode; + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED); + + return 1; + } + // Reset the filter if the IMU data is too old if (dtIMU > 0.2f) { + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); // that's all we can do here, return - return; + return 2; } // Check if we're on ground - this also sets static mode. @@ -2077,7 +2138,11 @@ void CheckAndBound() ResetPosition(); ResetHeight(); ResetStoredStates(); + + return 3; } + + return 0; } void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) @@ -2116,28 +2181,13 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float (&initvelNED)[3]) +void InitializeDynamic(float (&initvelNED)[3]) { - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } + // Clear the init flag + statesInitialised = false; - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements float initQuat[4]; @@ -2155,10 +2205,7 @@ void InitialiseFilter(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2187,3 +2234,51 @@ void InitialiseFilter(float (&initvelNED)[3]) summedDelVel.y = 0.0f; summedDelVel.z = 0.0f; } + +void InitialiseFilter(float (&initvelNED)[3]) +{ + //store initial lat,long and height + latRef = gpsLat; + lonRef = gpsLon; + hgtRef = gpsHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED); +} + +void ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 256aff7716..55e6eb12e0 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,6 +134,22 @@ enum GPS_FIX { GPS_FIX_3D = 3 }; +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool statesNaN; + bool covarianceNaN; + bool kalmanGainsNaN; +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -188,11 +204,22 @@ void ConstrainStates(); void ForceSymmetry(); -void CheckAndBound(); +int CheckAndBound(); void ResetPosition(); void ResetVelocity(); +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3]); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e46dffde3c..e75b844fd3 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -144,6 +145,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ + orb_advert_t _estimator_status_pub; /**< status of the estimator */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -260,6 +262,7 @@ FixedwingEstimator::FixedwingEstimator() : _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), + _estimator_status_pub(-1), _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -701,7 +704,67 @@ FixedwingEstimator::task_main() /** * CHECK IF THE INPUT DATA IS SANE */ - CheckAndBound(); + int check = CheckAndBound(); + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 3: + { + const char* str = "switching dynamic / static state"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + } + + // If non-zero, we got a problem + if (check) { + + struct ekf_status_report ekf_report; + + GetLastErrorState(&ekf_report); + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + rep.timestamp = hrt_absolute_time(); + + rep.states_nan = ekf_report.statesNaN; + rep.covariance_nan = ekf_report.covarianceNaN; + rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; + + // Copy all states or at least all that we can fit + int i = 0; + unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + while ((i < ekf_n_states) && (i < max_states)) { + + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } /** diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index fb24de8d16..4b31cc8a49 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -193,3 +193,6 @@ ORB_DEFINE(esc_status, struct esc_status_s); #include "topics/encoders.h" ORB_DEFINE(encoders, struct encoders_s); + +#include "topics/estimator_status.h" +ORB_DEFINE(estimator_status, struct estimator_status_report); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h new file mode 100644 index 0000000000..66ca030199 --- /dev/null +++ b/src/modules/uORB/topics/estimator_status.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 estimator_status.h + * Definition of the estimator_status_report uORB topic. + * + * @author Lorenz Meier + */ + +#ifndef ESTIMATOR_STATUS_H_ +#define ESTIMATOR_STATUS_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Estimator status report. + * + * This is a generic status report struct which allows any of the onboard estimators + * to write the internal state to the system log. + * + */ +struct estimator_status_report { + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot */ + float states[32]; /**< Internal filter states */ + float n_states; /**< Number of states effectively used */ + bool states_nan; /**< If set to true, one of the states is NaN */ + bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ + bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(estimator_status); + +#endif From d64c861ef84c6f1539dbb7cb1e5a5fd11e9f2656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:16:30 +0100 Subject: [PATCH 079/134] fixed wing estimator: Added trip command to test filter robustness --- .../fw_att_pos_estimator_main.cpp | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e75b844fd3..1d57f705a8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -1146,6 +1147,55 @@ void print_status() (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } +int trip_nan() { + + int ret = 0; + + // If system is not armed, inject a NaN value into the filter + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + struct actuator_armed_s armed; + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + if (armed.armed) { + warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + ret = 1; + } else { + + float nan_val = 0.0f / 0.0f; + + warnx("system not armed, tripping state vector with NaN values"); + states[5] = nan_val; + usleep(100000); + + // warnx("tripping covariance #1 with NaN values"); + // KH[2][2] = nan_val; // intermediate result used for covariance updates + // usleep(100000); + + // warnx("tripping covariance #2 with NaN values"); + // KHP[5][5] = nan_val; // intermediate result used for covariance updates + // usleep(100000); + + warnx("tripping covariance #3 with NaN values"); + P[3][3] = nan_val; // covariance matrix + usleep(100000); + + warnx("tripping Kalman gains with NaN values"); + Kfusion[0] = nan_val; // Kalman gains + usleep(100000); + + warnx("tripping stored states[0] with NaN values"); + storedStates[0][0] = nan_val; + usleep(100000); + + warnx("\nDONE - FILTER STATE:"); + print_status(); + } + + close(armed_sub); + return ret; +} + int fw_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) @@ -1192,6 +1242,17 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "trip")) { + if (estimator::g_estimator) { + int ret = trip_nan(); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } From 0874507a44347d10e10606061c411f0b0129dc5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:35:47 +0100 Subject: [PATCH 080/134] Added estimator status logging to sdlog2 --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ src/modules/uORB/topics/estimator_status.h | 1 - 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ad709d27dd..3f07eea533 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -794,6 +795,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; + struct estimator_status_report estimator_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -825,6 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; + struct log_ESTM_s log_ESTM; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +858,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int battery_sub; int telemetry_sub; int range_finder_sub; + int estimator_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -879,6 +883,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); thread_running = true; @@ -1243,6 +1248,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(DIST); } + /* --- ESTIMATOR STATUS --- */ + if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + log_msg.msg_type = LOG_ESTM_MSG; + unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); + memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); + memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); + log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; + log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; + log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; + log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fe500ad5f5..2b41755de9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -299,6 +299,16 @@ struct log_PARM_s { float value; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 132 +struct log_ESTM_s { + float s[32]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -331,6 +341,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), + LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 66ca030199..5530cdb210 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -67,7 +67,6 @@ struct estimator_status_report { bool states_nan; /**< If set to true, one of the states is NaN */ bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ - bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ }; From 29abf6db39dbf337d27c7dd85669934971444fef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:13:22 +0100 Subject: [PATCH 081/134] Fixed missing increment across states --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1d57f705a8..c9d75bce49 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -758,6 +758,7 @@ FixedwingEstimator::task_main() while ((i < ekf_n_states) && (i < max_states)) { rep.states[i] = ekf_report.states[i]; + i++; } if (_estimator_status_pub > 0) { From 8666ca53bf5b8eebbf60908c16273b711f80a19e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:13:56 +0100 Subject: [PATCH 082/134] Reducing VFR and HUD update rates, we do not need 100 Hz for 30 Hz human vision --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9f80219b1d..b7b5569453 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,6 +9,8 @@ mavlink start -r 10000 -d /dev/ttyACM0 # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10 From 0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:29:13 +0100 Subject: [PATCH 083/134] Guard against invalid states --- .../fw_att_pos_estimator/estimator.cpp | 67 ++++++++++++------- src/modules/fw_att_pos_estimator/estimator.h | 14 +++- 2 files changed, 53 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index bde46ba036..f03aa94741 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1674,34 +1674,49 @@ void ResetStoredStates() } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint64_t msec) +int RecallStates(float statesForFusion[n_states], uint64_t msec) { - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // The time delta can also end up as negative number, - // // since we might compare future to past or past to future - // // therefore cast to int64. - // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - // if (timeDelta < 0) { - // timeDelta = -timeDelta; - // } + int ret = 0; - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } - // else // otherwise output current state - // { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - // } + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // The time delta can also end up as negative number, + // since we might compare future to past or past to future + // therefore cast to int64. + int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + if (timeDelta < 0) { + timeDelta = -timeDelta; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (uint8_t i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + else // otherwise output current state + { + for (uint8_t i=0; i < n_states; i++) { + statesForFusion[i] = states[i]; + } + } + + return ret; } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 55e6eb12e0..c5a5e9d8d9 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -171,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces void StoreStates(uint64_t timestamp_ms); -// recall stste vector stored at closest time to the one specified by msec -void RecallStates(float statesForFusion[n_states], uint64_t msec); +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); void ResetStoredStates(); From 60728bb6a6211407a1db12e7c015272cf998d928 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:31:16 +0100 Subject: [PATCH 084/134] Now that the guard is updated disable time compensation again, but keep a guard against invalid state updates --- .../fw_att_pos_estimator/estimator.cpp | 64 ++++++++++--------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f03aa94741..46e405526e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1678,43 +1678,47 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec) { int ret = 0; - int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // The time delta can also end up as negative number, - // since we might compare future to past or past to future - // therefore cast to int64. - int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - if (timeDelta < 0) { - timeDelta = -timeDelta; - } + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) { + // if (isfinite(storedStates[i][bestStoreIndex])) { + // statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } else if (isfinite(states[i])) { + // statesForFusion[i] = states[i]; + // } else { + // // There is not much we can do here, except reporting the error we just + // // found. + // ret++; + // } + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) { - if (isfinite(storedStates[i][bestStoreIndex])) { - statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (isfinite(states[i])) { + if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { - // There is not much we can do here, except reporting the error we just - // found. ret++; } - } - else // otherwise output current state - { - for (uint8_t i=0; i < n_states; i++) { - statesForFusion[i] = states[i]; } - } + // } return ret; } From cb3a4f12670ce8d6cf608eb0ec97298a3bcb0919 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 085/134] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 409fa565f48ffe164b7332c9186a876b2771922a Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 086/134] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 903012bcff60e3d59f706bec7a920ce3e3aab754 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Mar 2014 14:14:58 +0100 Subject: [PATCH 087/134] Provide the wing-wing ESC an idle pulse to silence it --- ROMFS/px4fmu_common/init.d/3033_wingwing | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 6e1a531cfa..fff4b58df4 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -33,3 +33,6 @@ then fi set MIXER FMU_Q +# Provide ESC a constant 1000 us pulse +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 From 848c8364317d5db5d2accbeb788dfc67b5e02efa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Apr 2014 08:53:47 +0200 Subject: [PATCH 088/134] Robustify SF02/F parsing, adjust health checks and startup routine to known initialization time of the sensor --- src/drivers/sf0x/sf0x.cpp | 131 ++++++++++++++++++++++++-------------- 1 file changed, 83 insertions(+), 48 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 70cd1ab1e5..07163035b5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -276,34 +276,11 @@ SF0X::init() warnx("advert err"); } - /* attempt to get a measurement 5 times */ - while (ret != OK && i < 5) { - - if (measure()) { - ret = ERROR; - _sensor_ok = false; - } - - usleep(100000); - - if (collect()) { - ret = ERROR; - _sensor_ok = false; - - } else { - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; - } - - i++; - } - /* close the fd */ ::close(_fd); _fd = -1; out: - return ret; + return OK; } int @@ -376,6 +353,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { + /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -544,10 +522,16 @@ SF0X::collect() if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; + } else if (_linebuf_index > 0) { + /* increment to next read position */ + _linebuf_index++; } + /* the buffer for read chars is buflen minus null termination */ + unsigned readlen = sizeof(_linebuf) - 1; + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); if (ret < 0) { _linebuf[sizeof(_linebuf) - 1] = '\0'; @@ -562,19 +546,30 @@ SF0X::collect() } else { return -EAGAIN; } + } else if (ret == 0) { + return -EAGAIN; } - _linebuf_index += ret; - - if (_linebuf_index >= sizeof(_linebuf)) { - _linebuf_index = 0; - } + /* we did increment the index to the next position already, so just add the additional fields */ + _linebuf_index += (ret - 1); _last_read = hrt_absolute_time(); - if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read, reschedule ourselves */ + if (_linebuf_index < 1) { + /* we need at least the two end bytes to make sense of this string */ return -EAGAIN; + + } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + + if (_linebuf_index >= readlen - 1) { + /* we have a full buffer, but no line ending - abort */ + _linebuf_index = 0; + perf_count(_comms_errors); + return -ENOMEM; + } else { + /* incomplete read, reschedule ourselves */ + return -EAGAIN; + } } char *end; @@ -582,22 +577,56 @@ SF0X::collect() bool valid; /* enforce line ending */ - _linebuf[sizeof(_linebuf) - 1] = '\0'; + unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); + + _linebuf[lend] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(_linebuf, &end); - valid = true; + + /* we need to find a dot in the string, as we're missing the meters part else */ + valid = false; + + /* wipe out partially read content from last cycle(s), check for dot */ + for (int i = 0; i < (lend - 2); i++) { + if (_linebuf[i] == '\n') { + char buf[sizeof(_linebuf)]; + memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + } + + if (_linebuf[i] == '.') { + valid = true; + } + } + + if (valid) { + si_units = strtod(_linebuf, &end); + + /* we require at least 3 characters for a valid number */ + if (end > _linebuf + 3) { + valid = true; + } else { + si_units = -1.0f; + valid = false; + } + } } - debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); - /* done with this chunk, resetting */ + /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; + /* if its invalid, there is no reason to forward the value */ + if (!valid) { + perf_count(_comms_errors); + return -EINVAL; + } + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -689,7 +718,11 @@ SF0X::cycle() } if (OK != collect_ret) { - log("collection error"); + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL) { + log("collection error"); + } /* restart the measurement state machine */ start(); return; @@ -848,10 +881,10 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); - /* start the sensor polling at 2Hz */ + /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); } @@ -866,24 +899,26 @@ test() int ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + warnx("timed out"); + break; } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + break; } - warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("read #%u", i); + warnx("val: %0.3f m", (double)report.distance); + warnx("time: %lld", report.timestamp); } - /* reset the sensor polling to default rate */ + /* reset the sensor polling to the default rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set default poll rate"); + errx(1, "ERR: DEF RATE"); } errx(0, "PASS"); From 0ed4dd6577dc842ef151afd7751bd61744e2c77f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Apr 2014 16:16:24 +0200 Subject: [PATCH 089/134] Fixed log format --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f07eea533..13981ac544 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1258,7 +1258,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(DIST); + LOGBUFFER_WRITE_AND_COUNT(ESTM); } /* signal the other thread new data, but not yet unlock */ From fc757f9492a9495516e37abd6efc94c58904f1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 15:38:49 +0400 Subject: [PATCH 090/134] mavlink: is_published() fix --- src/modules/mavlink/mavlink_orb_subscription.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 4de7228324..d432edd2b4 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t) if (_updated) { orb_copy(_topic, _fd, _data); + _published = true; return true; } } From e075d05f579091fb9c605c856650cbfd1587a044 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:05:13 +0200 Subject: [PATCH 091/134] Move Pauls EKF into a class and instantiate only when / if needed. Checking for low memory conditions as we should. --- .../fw_att_pos_estimator/estimator.cpp | 189 +++------ src/modules/fw_att_pos_estimator/estimator.h | 237 +++++++---- .../fw_att_pos_estimator_main.cpp | 382 +++++++++--------- 3 files changed, 418 insertions(+), 390 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 46e405526e..3ce1ce56ec 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -2,85 +2,6 @@ #include -// Global variables -float KH[n_states][n_states]; // intermediate result used for covariance updates -float KHP[n_states][n_states]; // intermediate result used for covariance updates -float P[n_states][n_states]; // covariance matrix -float Kfusion[n_states]; // Kalman gains -float states[n_states]; // state matrix -Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) -Vector3f dVelIMU; -Vector3f dAngIMU; -float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) -uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity -float innovVelPos[6]; // innovation output -float varInnovVelPos[6]; // innovation variance output - -float velNED[3]; // North, East, Down velocity obs (m/s) -float posNE[2]; // North, East position obs (m) -float hgtMea; // measured height (m) -float posNED[3]; // North, East Down position (m) - -float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement -float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - -float innovMag[3]; // innovation output -float varInnovMag[3]; // innovation variance output -Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -float innovVtas; // innovation output -float varInnovVtas; // innovation variance output -float VtasMeas; // true airspeed measurement (m/s) -float latRef; // WGS-84 latitude of reference point (rad) -float lonRef; // WGS-84 longitude of reference point (rad) -float hgtRef; // WGS-84 height of reference point (m) -Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed - -// GPS input data variables -float gpsCourse; -float gpsVelD; -float gpsLat; -float gpsLon; -float gpsHgt; -uint8_t GPSstatus; - -float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps -uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored - -// Baro input -float baroHgt; - -bool statesInitialised = false; - -bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused -bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused -bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused -bool fuseMagData = false; // boolean true when magnetometer data is to be fused -bool fuseVtasData = false; // boolean true when airspeed data is to be fused - -bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) -bool staticMode = true; ///< boolean true if no position feedback is fused -bool useAirspeed = true; ///< boolean true if airspeed data is being used -bool useCompass = true; ///< boolean true if magnetometer data is being used - -struct ekf_status_report current_ekf_state; -struct ekf_status_report last_ekf_error; - -bool numericalProtection = true; - -static unsigned storeIndex = 0; - float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -185,7 +106,7 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -void UpdateStrapdownEquationsNED() +void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; float q00; @@ -247,7 +168,7 @@ void UpdateStrapdownEquationsNED() qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; // Normalise the quaternions and update the quaternion states - quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { float quatMagInv = 1.0f/quatMag; @@ -312,7 +233,7 @@ void UpdateStrapdownEquationsNED() ConstrainStates(); } -void CovariancePrediction(float dt) +void AttPosEKF::CovariancePrediction(float dt) { // scalars float windVelSigma; @@ -953,7 +874,7 @@ void CovariancePrediction(float dt) ConstrainVariances(); } -void FuseVelposNED() +void AttPosEKF::FuseVelposNED() { // declare variables used by fault isolation logic @@ -999,8 +920,8 @@ void FuseVelposNED() observation[5] = -(hgtMea); // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring R_OBS[0] = 0.04f + sq(velErr); R_OBS[1] = R_OBS[0]; R_OBS[2] = 0.08f + sq(velErr); @@ -1026,7 +947,7 @@ void FuseVelposNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; } // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; if (current_ekf_state.velHealth || current_ekf_state.velTimeout) { @@ -1175,7 +1096,7 @@ void FuseVelposNED() //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } -void FuseMagnetometer() +void AttPosEKF::FuseMagnetometer() { uint8_t obsIndex; uint8_t indexLimit; @@ -1482,7 +1403,7 @@ void FuseMagnetometer() ConstrainVariances(); } -void FuseAirspeed() +void AttPosEKF::FuseAirspeed() { float vn; float ve; @@ -1503,14 +1424,14 @@ void FuseAirspeed() // Need to check that it is flying before fusing airspeed data // Calculate the predicted airspeed - VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; - SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; float H_TAS[21]; for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; @@ -1611,7 +1532,7 @@ void FuseAirspeed() ConstrainVariances(); } -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1624,7 +1545,7 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) } } -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1637,13 +1558,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) } } -float sq(float valIn) +float AttPosEKF::sq(float valIn) { return valIn*valIn; } // Store states in a history array along with time stamp -void StoreStates(uint64_t timestamp_ms) +void AttPosEKF::StoreStates(uint64_t timestamp_ms) { for (unsigned i=0; i max) ? max : ((val < min) ? min : val); } -void ConstrainVariances() +void AttPosEKF::ConstrainVariances() { if (!numericalProtection) { return; @@ -1914,7 +1835,7 @@ void ConstrainVariances() } -void ConstrainStates() +void AttPosEKF::ConstrainStates() { if (!numericalProtection) { return; @@ -1971,7 +1892,7 @@ void ConstrainStates() } -void ForceSymmetry() +void AttPosEKF::ForceSymmetry() { if (!numericalProtection) { return; @@ -1989,7 +1910,7 @@ void ForceSymmetry() } } -bool FilterHealthy() +bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { return false; @@ -2012,7 +1933,7 @@ bool FilterHealthy() * This resets the position to the last GPS measurement * or to zero in case of static position. */ -void ResetPosition(void) +void AttPosEKF::ResetPosition(void) { if (staticMode) { states[7] = 0; @@ -2030,7 +1951,7 @@ void ResetPosition(void) * * This resets the height state with the last altitude measurements */ -void ResetHeight(void) +void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; @@ -2039,7 +1960,7 @@ void ResetHeight(void) /** * Reset the velocity state. */ -void ResetVelocity(void) +void AttPosEKF::ResetVelocity(void) { if (staticMode) { states[4] = 0.0f; @@ -2054,7 +1975,7 @@ void ResetVelocity(void) } -void FillErrorReport(struct ekf_status_report *err) +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { for (int i = 0; i < n_states; i++) { @@ -2069,7 +1990,7 @@ void FillErrorReport(struct ekf_status_report *err) err->hgtTimeout = current_ekf_state.hgtTimeout; } -bool StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { bool err = false; // check all states and covariance matrices @@ -2122,7 +2043,7 @@ bool StatesNaN(struct ekf_status_report *err_report) { * updated, but before any of the fusion steps are * executed. */ -int CheckAndBound() +int AttPosEKF::CheckAndBound() { // Store the old filter state @@ -2164,7 +2085,7 @@ int CheckAndBound() return 0; } -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2200,7 +2121,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) { // Clear the init flag @@ -2254,7 +2175,7 @@ void InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) { //store initial lat,long and height latRef = gpsLat; @@ -2266,7 +2187,7 @@ void InitialiseFilter(float (&initvelNED)[3]) InitializeDynamic(initvelNED); } -void ZeroVariables() +void AttPosEKF::ZeroVariables() { // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2292,12 +2213,12 @@ void ZeroVariables() memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } -void GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *state) { memcpy(state, ¤t_ekf_state, sizeof(state)); } -void GetLastErrorState(struct ekf_status_report *last_error) +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(last_error)); } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index c5a5e9d8d9..f43f4931af 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -48,85 +48,85 @@ void swap_var(float &d1, float &d2); const unsigned int n_states = 21; const unsigned int data_buffer_size = 50; -extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored -extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) -extern Vector3f dVelIMU; -extern Vector3f dAngIMU; +// extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored +// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +// extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +// extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +// extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +// extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) +// extern Vector3f dVelIMU; +// extern Vector3f dAngIMU; -extern float P[n_states][n_states]; // covariance matrix -extern float Kfusion[n_states]; // Kalman gains -extern float states[n_states]; // state matrix -extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps +// extern float P[n_states][n_states]; // covariance matrix +// extern float Kfusion[n_states]; // Kalman gains +// extern float states[n_states]; // state matrix +// extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps -extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) +// extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) -extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) -extern bool useAirspeed; // boolean true if airspeed data is being used -extern bool useCompass; // boolean true if magnetometer data is being used -extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity -extern float innovVelPos[6]; // innovation output -extern float varInnovVelPos[6]; // innovation variance output +// extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) +// extern bool useAirspeed; // boolean true if airspeed data is being used +// extern bool useCompass; // boolean true if magnetometer data is being used +// extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity +// extern float innovVelPos[6]; // innovation output +// extern float varInnovVelPos[6]; // innovation variance output -extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused -extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused -extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused -extern bool fuseMagData; // boolean true when magnetometer data is to be fused +// extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused +// extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused +// extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused +// extern bool fuseMagData; // boolean true when magnetometer data is to be fused -extern float velNED[3]; // North, East, Down velocity obs (m/s) -extern float posNE[2]; // North, East position obs (m) -extern float hgtMea; // measured height (m) -extern float posNED[3]; // North, East Down position (m) +// extern float velNED[3]; // North, East, Down velocity obs (m/s) +// extern float posNE[2]; // North, East position obs (m) +// extern float hgtMea; // measured height (m) +// extern float posNED[3]; // North, East Down position (m) -extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement +// extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +// extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +// extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement -extern float innovMag[3]; // innovation output -extern float varInnovMag[3]; // innovation variance output -extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -extern float innovVtas; // innovation output -extern float varInnovVtas; // innovation variance output -extern bool fuseVtasData; // boolean true when airspeed data is to be fused -extern float VtasMeas; // true airspeed measurement (m/s) -extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time -extern float latRef; // WGS-84 latitude of reference point (rad) -extern float lonRef; // WGS-84 longitude of reference point (rad) -extern float hgtRef; // WGS-84 height of reference point (m) -extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -extern float EAS2TAS; // ratio f true to equivalent airspeed +// extern float innovMag[3]; // innovation output +// extern float varInnovMag[3]; // innovation variance output +// extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +// extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +// extern float innovVtas; // innovation output +// extern float varInnovVtas; // innovation variance output +// extern bool fuseVtasData; // boolean true when airspeed data is to be fused +// extern float VtasMeas; // true airspeed measurement (m/s) +// extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +// extern float latRef; // WGS-84 latitude of reference point (rad) +// extern float lonRef; // WGS-84 longitude of reference point (rad) +// extern float hgtRef; // WGS-84 height of reference point (m) +// extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +// extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +// extern float EAS2TAS; // ratio f true to equivalent airspeed -// GPS input data variables -extern float gpsCourse; -extern float gpsVelD; -extern float gpsLat; -extern float gpsLon; -extern float gpsHgt; -extern uint8_t GPSstatus; +// // GPS input data variables +// extern float gpsCourse; +// extern float gpsVelD; +// extern float gpsLat; +// extern float gpsLon; +// extern float gpsHgt; +// extern uint8_t GPSstatus; -// Baro input -extern float baroHgt; +// // Baro input +// extern float baroHgt; -extern bool statesInitialised; -extern bool numericalProtection; +// extern bool statesInitialised; +// extern bool numericalProtection; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions -extern bool staticMode; +// extern bool staticMode; enum GPS_FIX { GPS_FIX_NOFIX = 0, @@ -150,6 +150,89 @@ struct ekf_status_report { bool kalmanGainsNaN; }; +class AttPosEKF { + +public: + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + + Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) + Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f dVelIMU; + Vector3f dAngIMU; + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + float innovVelPos[6]; // innovation output + float varInnovVelPos[6]; // innovation variance output + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float posNED[3]; // North, East Down position (m) + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float latRef; // WGS-84 latitude of reference point (rad) + float lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + + // GPS input data variables + float gpsCourse; + float gpsVelD; + float gpsLat; + float gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised = false; + + bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused + bool fuseMagData = false; // boolean true when magnetometer data is to be fused + bool fuseVtasData = false; // boolean true when airspeed data is to be fused + + bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode = true; ///< boolean true if no position feedback is fused + bool useAirspeed = true; ///< boolean true if airspeed data is being used + bool useCompass = true; ///< boolean true if magnetometer data is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection = true; + + unsigned storeIndex = 0; + + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -164,8 +247,6 @@ void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); -float sq(float valIn); - void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces @@ -190,15 +271,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -void eul2quat(float (&quat)[4], const float (&eul)[3]); +static void eul2quat(float (&quat)[4], const float (&eul)[3]); -void quat2eul(float (&eul)[3], const float (&quat)[4]); +static void quat2eul(float (&eul)[3], const float (&quat)[4]); -void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); void OnGroundCheck(); @@ -231,5 +316,15 @@ void FillErrorReport(struct ekf_status_report *err); void InitializeDynamic(float (&initvelNED)[3]); +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); + +}; + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c9d75bce49..20c5d37190 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -124,6 +124,16 @@ public: */ int start(); + /** + * Print the current status. + */ + void print_status(); + + /** + * Trip the filter by feeding it NaN values. + */ + int trip_nan(); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -199,6 +209,7 @@ private: param_t tas_delay_ms; } _parameter_handles; /**< handles for interesting parameters */ + AttPosEKF *_ekf; /** * Update our local parameter cache. @@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _initialized(false), _gps_initialized(false), - _mavlink_fd(-1) + _mavlink_fd(-1), + _ekf(new AttPosEKF()) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -384,6 +396,10 @@ void FixedwingEstimator::task_main() { + if (!_ekf) { + errx(1, "failed allocating EKF filter - out of RAM!"); + } + /* * do subscriptions */ @@ -414,23 +430,23 @@ FixedwingEstimator::task_main() parameters_update(); /* set initial filter state */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - statesInitialised = false; + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + _ekf->fuseHgtData = false; + _ekf->fuseMagData = false; + _ekf->fuseVtasData = false; + _ekf->statesInitialised = false; /* initialize measurement data */ - VtasMeas = 0.0f; + _ekf->VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; - dVelIMU.x = 0.0f; - dVelIMU.y = 0.0f; - dVelIMU.z = 0.0f; - dAngIMU.x = 0.0f; - dAngIMU.y = 0.0f; - dAngIMU.z = 0.0f; + _ekf->dVelIMU.x = 0.0f; + _ekf->dVelIMU.y = 0.0f; + _ekf->dVelIMU.z = 0.0f; + _ekf->dAngIMU.x = 0.0f; + _ekf->dAngIMU.y = 0.0f; + _ekf->dAngIMU.z = 0.0f; /* wakeup source(s) */ struct pollfd fds[2]; @@ -509,7 +525,7 @@ FixedwingEstimator::task_main() } last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3f; + _ekf.IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -521,20 +537,20 @@ FixedwingEstimator::task_main() // Always store data, independent of init status /* fill in last data set */ - dtIMU = deltaT; + _ekf->dtIMU = deltaT; - angRate.x = _gyro.x; - angRate.y = _gyro.y; - angRate.z = _gyro.z; + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; - accel.x = _accel.x; - accel.y = _accel.y; - accel.z = _accel.z; + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; + _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + _ekf->lastAngRate = angRate; + _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + _ekf->lastAccel = accel; #else @@ -563,20 +579,20 @@ FixedwingEstimator::task_main() // Always store data, independent of init status /* fill in last data set */ - dtIMU = deltaT; + _ekf->dtIMU = deltaT; - angRate.x = _sensor_combined.gyro_rad_s[0]; - angRate.y = _sensor_combined.gyro_rad_s[1]; - angRate.z = _sensor_combined.gyro_rad_s[2]; + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - accel.x = _sensor_combined.accelerometer_m_s2[0]; - accel.y = _sensor_combined.accelerometer_m_s2[1]; - accel.z = _sensor_combined.accelerometer_m_s2[2]; + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; @@ -597,7 +613,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); - VtasMeas = _airspeed.true_airspeed_m_s; + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; } else { @@ -622,24 +638,24 @@ FixedwingEstimator::task_main() /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { - ResetPosition(); - ResetVelocity(); - ResetStoredStates(); + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); } /* fuse GPS updates */ //_gps.timestamp / 1e3; - GPSstatus = _gps.fix_type; - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; newDataGps = true; } @@ -652,10 +668,10 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude - _baro_ref; // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; + _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -671,27 +687,27 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. // 0.001f - magData.x = _mag.x; - magBias.x = 0.000001f; // _mag_offsets.x_offset + _ekf->magData.x = _mag.x; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - magData.y = _mag.y; - magBias.y = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.y = _mag.y; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - magData.z = _mag.z; - magBias.z = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.z = _mag.z; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f - magData.x = _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.000001f; // _mag_offsets.x_offset + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - magData.y = _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - magData.z = _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #endif @@ -705,7 +721,7 @@ FixedwingEstimator::task_main() /** * CHECK IF THE INPUT DATA IS SANE */ - int check = CheckAndBound(); + int check = _ekf->CheckAndBound(); switch (check) { case 0: @@ -739,7 +755,7 @@ FixedwingEstimator::task_main() struct ekf_status_report ekf_report; - GetLastErrorState(&ekf_report); + _ekf->GetLastErrorState(&ekf_report); struct estimator_status_report rep; memset(&rep, 0, sizeof(rep)); @@ -779,16 +795,16 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (GPSstatus == 3)) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; double lat = _gps.lat * 1e-7; double lon = _gps.lon * 1e-7; float alt = _gps.alt * 1e-3; - InitialiseFilter(velNED); + _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -799,7 +815,7 @@ FixedwingEstimator::task_main() // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref = _baro.altitude; - baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe @@ -808,24 +824,24 @@ FixedwingEstimator::task_main() _gps_initialized = true; - } else if (!statesInitialised) { - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - posNED[0] = 0.0f; - posNED[1] = 0.0f; - posNED[2] = 0.0f; + } else if (!_ekf->statesInitialised) { + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - InitialiseFilter(velNED); + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; + _ekf->InitialiseFilter(_ekf->velNED); } } // If valid IMU data and states initialised, predict states and covariances - if (statesInitialised) { + if (_ekf->statesInitialised) { // Run the strapdown INS equations every IMU update - UpdateStrapdownEquationsNED(); + _ekf->UpdateStrapdownEquationsNED(); #if 0 // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; @@ -842,20 +858,20 @@ FixedwingEstimator::task_main() #endif // store the predicted states for subsequent use by measurement fusion - StoreStates(IMUmsec); + _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction - OnGroundCheck(); + _ekf->OnGroundCheck(); // sum delta angles and time used by covariance prediction - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + dVelIMU; - dt += dtIMU; + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); + if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng = _ekf->summedDelAng.zero(); + _ekf->summedDelVel = _ekf->summedDelVel.zero(); dt = 0.0f; } @@ -865,79 +881,79 @@ FixedwingEstimator::task_main() // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; // set fusion flags - fuseVelData = true; - fusePosData = true; + _ekf->fuseVelData = true; + _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); - } else if (statesInitialised) { + } else if (_ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - posNED[0] = 0.0f; - posNED[1] = 0.0f; - posNED[2] = 0.0f; + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; // set fusion flags - fuseVelData = true; - fusePosData = true; + _ekf->fuseVelData = true; + _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); } else { - fuseVelData = false; - fusePosData = false; + _ekf->fuseVelData = false; + _ekf->fusePosData = false; } - if (newAdsData && statesInitialised) { + if (newAdsData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; - fuseHgtData = true; + _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); } else { - fuseHgtData = false; + _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements - if (newDataMag && statesInitialised) { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data } else { - fuseMagData = false; + _ekf->fuseMagData = false; } - if (statesInitialised) FuseMagnetometer(); + if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); // Fuse Airspeed Measurements - if (newAdsData && statesInitialised && VtasMeas > 8.0f) { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); } else { - fuseVtasData = false; + _ekf->fuseVtasData = false; } // Publish results @@ -954,7 +970,7 @@ FixedwingEstimator::task_main() // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); @@ -962,10 +978,10 @@ FixedwingEstimator::task_main() _att.R[i][j] = R(i, j); _att.timestamp = last_sensor_timestamp; - _att.q[0] = states[0]; - _att.q[1] = states[1]; - _att.q[2] = states[2]; - _att.q[3] = states[3]; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; _att.q_valid = true; _att.R_valid = true; @@ -974,13 +990,13 @@ FixedwingEstimator::task_main() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = angRate.x - states[10]; - _att.pitchspeed = angRate.y - states[11]; - _att.yawspeed = angRate.z - states[12]; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; // gyro offsets - _att.rate_offsets[0] = states[10]; - _att.rate_offsets[1] = states[11]; - _att.rate_offsets[2] = states[12]; + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -993,20 +1009,15 @@ FixedwingEstimator::task_main() } } - if (!isfinite(states[0])) { - print_status(); - _exit(0); - } - if (_gps_initialized) { _local_pos.timestamp = last_sensor_timestamp; - _local_pos.x = states[7]; - _local_pos.y = states[8]; - _local_pos.z = states[9]; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + _local_pos.z = _ekf->states[9]; - _local_pos.vx = states[4]; - _local_pos.vy = states[5]; - _local_pos.vz = states[6]; + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; @@ -1107,9 +1118,10 @@ FixedwingEstimator::start() return OK; } -void print_status() +void +FixedwingEstimator::print_status() { - math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); @@ -1125,30 +1137,30 @@ void print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int trip_nan() { +int FixedwingEstimator::trip_nan() { int ret = 0; @@ -1166,7 +1178,7 @@ int trip_nan() { float nan_val = 0.0f / 0.0f; warnx("system not armed, tripping state vector with NaN values"); - states[5] = nan_val; + _ekf->states[5] = nan_val; usleep(100000); // warnx("tripping covariance #1 with NaN values"); @@ -1178,15 +1190,15 @@ int trip_nan() { // usleep(100000); warnx("tripping covariance #3 with NaN values"); - P[3][3] = nan_val; // covariance matrix + _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); warnx("tripping Kalman gains with NaN values"); - Kfusion[0] = nan_val; // Kalman gains + _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); warnx("tripping stored states[0] with NaN values"); - storedStates[0][0] = nan_val; + _ekf->storedStates[0][0] = nan_val; usleep(100000); warnx("\nDONE - FILTER STATE:"); @@ -1234,7 +1246,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator) { warnx("running"); - print_status(); + estimator::g_estimator->print_status(); exit(0); @@ -1245,7 +1257,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "trip")) { if (estimator::g_estimator) { - int ret = trip_nan(); + int ret = estimator::g_estimator->trip_nan(); exit(ret); From 2b6a9c5122008ca47cf7524b6887d7de9b0b8a5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:07:58 +0200 Subject: [PATCH 092/134] Removed a bunch of commented out things that we will not need any more. --- src/modules/fw_att_pos_estimator/estimator.h | 75 -------------------- 1 file changed, 75 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index f43f4931af..821392399a 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -48,81 +48,6 @@ void swap_var(float &d1, float &d2); const unsigned int n_states = 21; const unsigned int data_buffer_size = 50; -// extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored -// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -// extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -// extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -// extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -// extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) -// extern Vector3f dVelIMU; -// extern Vector3f dAngIMU; - -// extern float P[n_states][n_states]; // covariance matrix -// extern float Kfusion[n_states]; // Kalman gains -// extern float states[n_states]; // state matrix -// extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - -// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - -// extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - -// extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) -// extern bool useAirspeed; // boolean true if airspeed data is being used -// extern bool useCompass; // boolean true if magnetometer data is being used -// extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity -// extern float innovVelPos[6]; // innovation output -// extern float varInnovVelPos[6]; // innovation variance output - -// extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused -// extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused -// extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused -// extern bool fuseMagData; // boolean true when magnetometer data is to be fused - -// extern float velNED[3]; // North, East, Down velocity obs (m/s) -// extern float posNE[2]; // North, East position obs (m) -// extern float hgtMea; // measured height (m) -// extern float posNED[3]; // North, East Down position (m) - -// extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -// extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -// extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - -// extern float innovMag[3]; // innovation output -// extern float varInnovMag[3]; // innovation variance output -// extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -// extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -// extern float innovVtas; // innovation output -// extern float varInnovVtas; // innovation variance output -// extern bool fuseVtasData; // boolean true when airspeed data is to be fused -// extern float VtasMeas; // true airspeed measurement (m/s) -// extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time -// extern float latRef; // WGS-84 latitude of reference point (rad) -// extern float lonRef; // WGS-84 longitude of reference point (rad) -// extern float hgtRef; // WGS-84 height of reference point (m) -// extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -// extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -// extern float EAS2TAS; // ratio f true to equivalent airspeed - -// // GPS input data variables -// extern float gpsCourse; -// extern float gpsVelD; -// extern float gpsLat; -// extern float gpsLon; -// extern float gpsHgt; -// extern uint8_t GPSstatus; - -// // Baro input -// extern float baroHgt; - -// extern bool statesInitialised; -// extern bool numericalProtection; - const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions From fcd31b03686ae9a6e04f294044420d09e8e6a2cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:14:23 +0200 Subject: [PATCH 093/134] Reduced the number of states to 10 to avoid killing the logging system --- src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b41755de9..22a6958720 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -302,12 +302,19 @@ struct log_PARM_s { /* --- ESTM - ESTIMATOR STATUS --- */ #define LOG_ESTM_MSG 132 struct log_ESTM_s { - float s[32]; + float s[10]; uint8_t n_states; uint8_t states_nan; uint8_t covariance_nan; uint8_t kalman_gain_nan; }; +// struct log_ESTM_s { +// float s[32]; +// uint8_t n_states; +// uint8_t states_nan; +// uint8_t covariance_nan; +// uint8_t kalman_gain_nan; +// }; #pragma pack(pop) @@ -341,7 +348,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 88cf841f00b499792780195de63018b3bd49f683 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:18:17 +0200 Subject: [PATCH 094/134] Bump RC timeout for all cases to half a second --- src/modules/commander/commander.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..3720b5a3b3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; From 1e25ceb085a8ca5cd53825a2eb30d9cf69c3a8d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:47:30 +0200 Subject: [PATCH 095/134] Create EKF object in right context --- src/modules/fw_att_pos_estimator/estimator.cpp | 11 ++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 4 ++++ .../fw_att_pos_estimator_main.cpp | 4 +++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 3ce1ce56ec..7ab06e85d3 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -106,7 +106,16 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -void AttPosEKF::UpdateStrapdownEquationsNED() +AttPosEKF::AttPosEKF() +{ + +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; float q00; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 821392399a..7edb3c714f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -78,6 +78,10 @@ struct ekf_status_report { class AttPosEKF { public: + + AttPosEKF(); + ~AttPosEKF(); + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 20c5d37190..840cd585ec 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -292,7 +292,7 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _gps_initialized(false), _mavlink_fd(-1), - _ekf(new AttPosEKF()) + _ekf(nullptr) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -396,6 +396,8 @@ void FixedwingEstimator::task_main() { + _ekf = new AttPosEKF(); + if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); } From c6d98a32f83383e6204fd6cefbfcc1fd7e1cf159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:27:43 +0200 Subject: [PATCH 096/134] Proper failsafe handling onboard, including throttle failsafe condition if enabled --- src/modules/px4iofirmware/controls.c | 36 ++++++++++++++++++---------- src/modules/px4iofirmware/protocol.h | 9 +++---- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..b860fc587d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); @@ -261,8 +259,20 @@ controls_tick() { if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; + } + + if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); @@ -312,6 +322,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -322,27 +337,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - - /* Set channel count to zero */ - r_raw_rc_count = 0; - } - /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c5297..b69f681070 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -164,10 +164,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ -#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ From 64ffafb48ee45452070dfb37be8d0de6098915a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:28:01 +0200 Subject: [PATCH 097/134] Only publish RC inputs if we have seen some valid inputs at some point --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From fb44ad8e22f2a0862f1d7bda66643a6336247024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:29:17 +0200 Subject: [PATCH 098/134] Simplify the failsafe handling, reduce 3 params to one --- src/modules/sensors/sensor_params.c | 28 +++------------------------- src/modules/sensors/sensors.cpp | 26 +++++++------------------- 2 files changed, 10 insertions(+), 44 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a0..e4564aa25c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - -/** - * Failsafe channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_CH, 0); - -/** - * Failsafe channel mode. - * - * 0 = too low means signal loss, - * 1 = too high means signal loss - * - * @min 0 - * @max 1 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); - /** * Failsafe channel PWM threshold. * - * @min 0 - * @max 1 + * @min 800 + * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); +PARAM_DEFINE_FLOAT(RC_FS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f8..2442acd6b0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,8 +263,6 @@ private: float rc_scale_yaw; float rc_scale_flaps; - int rc_fs_ch; - int rc_fs_mode; float rc_fs_thr; float battery_voltage_scaling; @@ -313,8 +311,6 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; - param_t rc_fs_ch; - param_t rc_fs_mode; param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -531,8 +527,6 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); /* gyro offsets */ @@ -689,8 +683,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); - param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); - param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ @@ -1312,19 +1304,15 @@ Sensors::rc_poll() manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) + if (rc_input.channel_count < 4) { return; + } - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; - } + /* check for failsafe */ + if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + /* do not publish manual control setpoints when there are none */ + return; } unsigned channel_limit = rc_input.channel_count; From 797698a7a114032c7eea62c5f48f2b229ca973b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:34:35 +0200 Subject: [PATCH 099/134] Trigger failsafe action also on failsafe flag --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2442acd6b0..4c34d853fc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,7 +1309,7 @@ Sensors::rc_poll() } /* check for failsafe */ - if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { /* do not publish manual control setpoints when there are none */ return; From 9123ebce8cbc618899ece31d31c97e022038beb2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:45:02 +0200 Subject: [PATCH 100/134] px4io: Allow RC failsafe detection as valid feature --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa8..42b863b539 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -175,7 +175,8 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ + PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 9a0b2b7610d39f88b627046e0d90f66aada1e88f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:02:22 +0200 Subject: [PATCH 101/134] Make throttle failsafe depend on the failsafe threshold parameter. Make the parameter optional (no harm if not found). --- src/drivers/px4io/px4io.cpp | 17 ++++++++++++++++- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/protocol.h | 21 +++++++++++---------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++-- 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8eee1cbca0..f6125d273d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -944,8 +944,23 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); } + + /* send RC throttle failsafe value to IO */ + float failsafe_param_val; + param_t failsafe_param = param_find("RC_FS_THR"); + + if (failsafe_param > 0) + + param_get(failsafe_param, &failsafe_param_val); + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed"); + } + } + } } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b860fc587d..356fe44cd5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -264,10 +264,10 @@ controls_tick() { scaled = -scaled; } - if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + if (mapped == 3 && r_setup_rc_thr_failsafe) { /* throttle failsafe detection */ - if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || - ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b69f681070..a978d483a4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -168,7 +168,6 @@ #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ #define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ #define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -202,13 +201,15 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + /* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ -#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ -#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ -#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* 12 occupied by CRC */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -218,10 +219,10 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5b..88ad793980 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42b863b539..6752e7b4b3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,14 +169,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ - PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 5e0d687b566022c12270f68facbf7ca35f62306c Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 102/134] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 745ef4f4856af60aca5625860e5e3f25ea189dc9 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 103/134] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 09d106432749ec02866241863eb912f05b903e64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:06:07 +0200 Subject: [PATCH 104/134] px4io: Remove unused variable --- src/modules/px4iofirmware/controls.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..f56f630bce 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); From 3e4841b6fe2d8d6d06b167be49cbe76ab7e04a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:10:41 +0200 Subject: [PATCH 105/134] px4io: Guard against the RC failsafe value of channel 5 causing a manual override action if set to manual in failsafe --- src/modules/px4iofirmware/controls.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f56f630bce..aae7bb951a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -310,6 +310,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -320,27 +325,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - } - - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - /* Set channel count to zero */ r_raw_rc_count = 0; + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -363,10 +365,10 @@ controls_tick() { mixer_tick(); } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } From 73d04f7a37ba52ce1891e740db554557bd71940a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:15:22 +0200 Subject: [PATCH 106/134] px4io driver: Only publish RC signal if it was at least once valid. --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From 3b5e6f98338fded2cbe7be1c301dc2698f7239aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:28:07 +0200 Subject: [PATCH 107/134] sensors and px4io driver: Guard against failsafe trigger for inverted remotes --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6125d273d..a30bfb2dec 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -948,8 +948,8 @@ PX4IO::task_main() } /* send RC throttle failsafe value to IO */ - float failsafe_param_val; - param_t failsafe_param = param_find("RC_FS_THR"); + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); if (failsafe_param > 0) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index e4564aa25c..14f7ac812f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -675,4 +675,4 @@ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 0); +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4c34d853fc..44a91ca67a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,7 +263,7 @@ private: float rc_scale_yaw; float rc_scale_flaps; - float rc_fs_thr; + int32_t rc_fs_thr; float battery_voltage_scaling; float battery_current_scaling; @@ -527,7 +527,7 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { /* do not publish manual control setpoints when there are none */ return; } From ec2197fd1b228d8eb4855c49d5f9b1365685d01c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Apr 2014 13:02:44 +1100 Subject: [PATCH 108/134] uploader: ignore bad character encodings for older bootloaders this prevents the uploader from throwing an exception with older bootloaders --- Tools/px_uploader.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index e4a8b3c054..985e6ffd9d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -389,18 +389,22 @@ class uploader(object): self.otp_pid = self.otp[12:8:-1] self.otp_coa = self.otp[32:160] # show user: - print("type: " + self.otp_id.decode('Latin-1')) - print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) - print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) - print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) - print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) - print("sn: ", end='') - for byte in range(0,12,4): - x = self.__getSN(byte) - x = x[::-1] # reverse the bytes - self.sn = self.sn + x - print(binascii.hexlify(x).decode('Latin-1'), end='') # show user - print('') + try: + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user + print('') + except Exception: + # ignore bad character encodings + pass print("erase...") self.__erase() From ea5279389f2b13110735083f511afb630ef5e3d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:16:51 +1100 Subject: [PATCH 109/134] uORB: added an ORB topic for system_power holds power supply state and 5V rail voltage on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/system_power.h | 71 ++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/system_power.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a49..c8a589bb7c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 0000000000..7763b60043 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 system_power.h + * + * Definition of the system_power voltage and power status uORB topic. + */ + +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * voltage and power supply status + */ +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif From 6ea22c8c079db8633d663cbf8ca39b81a434a040 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:17:25 +1100 Subject: [PATCH 110/134] adc: publish system_power ORB topic from ADC --- src/drivers/stm32/adc/adc.cpp | 47 ++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 0b8a275e6a..3a60d2cae1 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include @@ -64,6 +65,8 @@ #include #include +#include + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -119,6 +122,8 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ + orb_advert_t _to_system_power; + /** work trampoline */ static void _tick_trampoline(void *arg); @@ -134,13 +139,16 @@ private: */ uint16_t _sample(unsigned channel); + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); }; ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), _channel_count(0), - _samples(nullptr) + _samples(nullptr), + _to_system_power(0) { _debug_enabled = true; @@ -290,6 +298,43 @@ ADC::_tick() /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power > 0) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } uint16_t From 30d1ce3a51caafe959fc048e4d7ca6147bd2ccd6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 11 Mar 2014 12:42:46 +1100 Subject: [PATCH 111/134] hmc5883: properly reset mag to normal state on calibration fail and add current output in "hmc5883 info" --- src/drivers/hmc5883/hmc5883.cpp | 36 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4c85c0cdaa..cb97354ecc 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -169,6 +169,8 @@ private: int _bus; /**< the bus the device is connected to */ + struct mag_report _last_report; /**< used for info() */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -870,6 +872,8 @@ HMC5883::collect() } } + _last_report = new_report; + /* post a report to the ring */ if (_reports->force(&new_report)) { perf_count(_buffer_overflows); @@ -1042,32 +1046,29 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set back to normal mode */ - /* Set to 1.1 Gauss */ - if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); - goto out; - } - - if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); - goto out; - } - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; mscale_previous.z_scale = scaling[2]; - if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); - goto out; - } - ret = OK; out: + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + } + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + } + if (ret == OK) { if (!check_scale()) { warnx("mag scale calibration successfully finished."); @@ -1221,6 +1222,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, From f1f9f452c0225b7ebf65007505e6d74e57216cc1 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 22 Mar 2014 16:57:32 -0700 Subject: [PATCH 112/134] tone_alarm: Added arming failure tone --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index cb5fd53a5f..147d7123a4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -147,6 +147,7 @@ enum { TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, + TONE_ARMING_FAILURE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f36f2091e3..8ed0de58c6 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Sat, 5 Apr 2014 12:42:32 +0200 Subject: [PATCH 113/134] sdlog2: Add system power to logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac544..7a984821f2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -796,6 +796,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct system_power_s system_power; + struct servorail_status_s servorail_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -828,6 +830,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; + struct log_PWR_s log_PWR; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -859,6 +862,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int system_power_sub; + int servorail_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -884,6 +889,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); + subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); thread_running = true; @@ -1226,6 +1233,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- SYSTEM POWER RAILS --- */ + if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + log_msg.msg_type = LOG_PWR_MSG; + log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; + log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; + log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; + log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; + log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; + + /* copy servo rail status topic here too */ + orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); + log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + + LOGBUFFER_WRITE_AND_COUNT(PWR); + } + /* --- TELEMETRY --- */ if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { log_msg.msg_type = LOG_TELE_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 22a6958720..93a150dbea 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -316,6 +316,19 @@ struct log_ESTM_s { // uint8_t kalman_gain_nan; // }; +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 133 +struct log_PWR_s { + float 5v_peripherals; + float 5v_servo_rail; + float 5v_servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -349,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From f21ce7e50cf4e9f77fb7d26508c989ed2ff1f300 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 12:56:15 +0200 Subject: [PATCH 114/134] Compile hotfix for master --- .../fw_att_pos_estimator/estimator.cpp | 17 ++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 30 +++++++++---------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 7ab06e85d3..c312173938 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -106,7 +106,22 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0) { } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 7edb3c714f..e62f1a98a5 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -108,7 +108,7 @@ public: Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -127,8 +127,8 @@ public: float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + float EAS2TAS; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; @@ -141,25 +141,25 @@ public: // Baro input float baroHgt; - bool statesInitialised = false; + bool statesInitialised; - bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused - bool fuseMagData = false; // boolean true when magnetometer data is to be fused - bool fuseVtasData = false; // boolean true when airspeed data is to be fused + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused - bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode = true; ///< boolean true if no position feedback is fused - bool useAirspeed = true; ///< boolean true if airspeed data is being used - bool useCompass = true; ///< boolean true if magnetometer data is being used + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; - bool numericalProtection = true; + bool numericalProtection; - unsigned storeIndex = 0; + unsigned storeIndex; void UpdateStrapdownEquationsNED(); From 06d5c6ad285a46cfb59ce07bc3cd49507305a997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 13:03:52 +0200 Subject: [PATCH 115/134] sdlog2: Compile fixes for system power logging --- src/modules/sdlog2/sdlog2.c | 8 +++++--- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7a984821f2..9f2fca4465 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,6 +84,8 @@ #include #include #include +#include +#include #include #include @@ -1236,7 +1238,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SYSTEM POWER RAILS --- */ if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; - log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; @@ -1245,8 +1247,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); - log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93a150dbea..d629259c0c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -319,9 +319,9 @@ struct log_ESTM_s { /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 133 struct log_PWR_s { - float 5v_peripherals; - float 5v_servo_rail; - float 5v_servo_rssi; + float peripherals_5v; + float servo_rail_5v; + float servo_rssi_5v; uint8_t usb_ok; uint8_t brick_ok; uint8_t servo_ok; @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From 00c82f0836388ffa1e1c4a5827eefe28c0521df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:18:20 +1100 Subject: [PATCH 116/134] Merged airspeed changes --- src/drivers/meas_airspeed/meas_airspeed.cpp | 62 ++++++++++++++++++++- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2c3efdc35f..2324475ceb 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include @@ -121,6 +122,14 @@ protected: virtual int collect(); math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -129,10 +138,11 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -207,6 +217,9 @@ MEASAirspeed::collect() diff_press_pa = 0.0f; } + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa); + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -287,6 +300,49 @@ MEASAirspeed::cycle() USEC2TICK(CONVERSION_INTERVAL)); } +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 70.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + /** * Local functions in support of the shell command. */ From 9719af623d92ceab0c5eebe21906b4d5cf515682 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 13:48:37 +1100 Subject: [PATCH 117/134] Merged voltage compensation --- src/drivers/meas_airspeed/meas_airspeed.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ceb..0b7986383f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - float temperature = ((200 * dT_raw) / 2047) - 50; + float temperature = ((200.0f * dT_raw) / 2047) - 50; /* calculate differential pressure. As its centered around 8000 * and can go positive or negative, enforce absolute value @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: [PATCH 118/134] Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 ++++++++++++++----- .../uORB/topics/differential_pressure.h | 9 ++-- 2 files changed, 43 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383f..58b128948f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6e..01e14cda9e 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 3da219c3db638e0a57d18e892575df13d8c11f47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:14:03 +0200 Subject: [PATCH 119/134] Update airspeed calibration routine to account for new signedness options --- src/modules/commander/airspeed_calibration.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a76..c8c7a42e79 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd) if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) From be38372be17925e14fa864ad26e8f487a402d46f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:28:44 +0200 Subject: [PATCH 120/134] sf0x driver: Stop emitting error messages once there is no hope this sensor will recover - continue to output error messages if the errors are just intermittent --- src/drivers/sf0x/sf0x.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 07163035b5..a0cf98340a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -124,6 +124,8 @@ private: orb_advert_t _range_finder_topic; + unsigned _consecutive_fail_count; + perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _last_read(0), _range_finder_topic(-1), + _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) @@ -720,12 +723,17 @@ SF0X::cycle() if (OK != collect_ret) { /* we know the sensor needs about four seconds to initialize */ - if (hrt_absolute_time() > 5 * 1000 * 1000LL) { - log("collection error"); + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + log("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; + /* restart the measurement state machine */ start(); return; + } else { + /* apparently success */ + _consecutive_fail_count = 0; } /* next phase is measurement */ From 671d35f67ccae5c35ef4fcb573f5fa52395601c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:05:58 +0200 Subject: [PATCH 121/134] Fix logic for S.Bus failsafe detection --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 44a91ca67a..c910062620 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From de7a6b057f5ced2cf118a919912327f878492cdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:08:28 +0200 Subject: [PATCH 122/134] Cleanups on system power logging format --- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/sdlog2/sdlog2_messages.h | 64 ++++++++++++---------------- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f2fca4465..d2cf6d662b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1248,7 +1248,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d629259c0c..354bb08e8d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -277,6 +277,29 @@ struct log_TELE_s { uint8_t txbuf; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 23 +struct log_ESTM_s { + float s[10]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 24 +struct log_PWR_s { + float peripherals_5v; + float servo_rail_5v; + float servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -299,36 +322,6 @@ struct log_PARM_s { float value; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 132 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; -// struct log_ESTM_s { -// float s[32]; -// uint8_t n_states; -// uint8_t states_nan; -// uint8_t covariance_nan; -// uint8_t kalman_gain_nan; -// }; - -/* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 133 -struct log_PWR_s { - float peripherals_5v; - float servo_rail_5v; - float servo_rssi_5v; - uint8_t usb_ok; - uint8_t brick_ok; - uint8_t servo_ok; - uint8_t low_power_rail_overcurrent; - uint8_t high_power_rail_overcurrent; -}; - #pragma pack(pop) /* construct list of all message formats */ @@ -355,17 +348,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), /* system-level messages, ID >= 0x80 */ - // FMT: don't write format of format message, it's useless + /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), - LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PARM, "Nf", "Name,Value") }; -static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); +static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]); #endif /* SDLOG2_MESSAGES_H_ */ From 7b95d36405cb63b53fd1fea2c25e29aedca5a3a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:45:23 +0200 Subject: [PATCH 123/134] navigator hotfix: Increase acceptance range for yaw setpoints. --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1ba..5a94b66717 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } From fc39af08a1d6673aa727a84b17afd6c4485dff19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:02:37 +0200 Subject: [PATCH 124/134] airspeed: Prevent the filter from overshooting into the negative airspeed range --- src/drivers/meas_airspeed/meas_airspeed.cpp | 6 ++++++ .../mathlib/math/filter/LowPassFilter2p.cpp | 7 ++++++- .../mathlib/math/filter/LowPassFilter2p.hpp | 20 +++++++++++++++---- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 58b128948f..1ad383ee0b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,6 +258,12 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + + /* the dynamics of the filter can make it overshoot into the negative range */ + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce3..6f640c9f9a 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4e..74cd5d78c0 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; From 024de1fec431fbd065aeb31035245e7851450a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:04:36 +0200 Subject: [PATCH 125/134] Remove unwanted colon --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad793980..4db9484848 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) From 262485a5e87ccdc8ff645ba45992bdbe13363fab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:09:48 +0200 Subject: [PATCH 126/134] px4io: Typo fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a30bfb2dec..e318e206ad 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,7 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; From e6d48c4f3276af252315118286556b3a000274b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:12:21 +0200 Subject: [PATCH 127/134] Fix failsafe assignment in sensors app --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c910062620..b171b995d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From 6319ec2036c52f39a6fded6480836fe79e3ba35f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:34 +0200 Subject: [PATCH 128/134] Add celsius air temperature field to airspeed --- src/modules/uORB/topics/airspeed.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758fd..d2ee754cdf 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** From c17201afbff06f2be278e202d23eb136f38d1ae1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:54 +0200 Subject: [PATCH 129/134] Log air temperature --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d2cf6d662b..e62b0fafc6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; LOGBUFFER_WRITE_AND_COUNT(AIRS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 354bb08e8d..a1a5856bc0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -174,6 +174,7 @@ struct log_OUT0_s { struct log_AIRS_s { float indicated_airspeed; float true_airspeed; + float air_temperature_celsius; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ @@ -338,7 +339,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), From a1a4013d02ca7ec3f62c6c3f2e4b95181f365c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:24:11 +0200 Subject: [PATCH 130/134] Populate air temperature field --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b171b995d2..4083b8b4f5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1025,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celcius); + raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { From 4a0c6600887e900932f6888f1b8948816a1f00b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:06:21 +0200 Subject: [PATCH 131/134] Do not make minimum airspeed assumptions, as we can trust our digital sensor a bit. A blocked pitot also most likely will result in more than just 6.5 m/s airspeed and so the check is based on a bunch of weak assumptions --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f48..522e8c3e6d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,14 +660,13 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } float airspeed_scaling = _parameters.airspeed_trim / airspeed; From ab60b13b6dbcf636b1889d2150d96aff8b26cfc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:26:17 +0200 Subject: [PATCH 132/134] fw_att_controller: Forcing actuator scaling to at least minimum speed --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 522e8c3e6d..2f84dc963b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -669,8 +669,15 @@ FixedwingAttitudeControl::task_main() airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; From 7c4f1c90dc441e57d9af55beb33bb6d91ece0c90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 14:57:45 +0400 Subject: [PATCH 133/134] mavlink_receiver: fixed bug in manual control publication, minor refactoring --- src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++------------ src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670a..3ec40ee0a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7a..72ce4560fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; From e4cfdb4f9f30e0eef36f9e18e8d656d57994141e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Apr 2014 19:34:32 +0200 Subject: [PATCH 134/134] mavlink: Add manual SP subscription --- src/modules/mavlink/mavlink_receiver.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 72ce4560fb..8ccb2a0354 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited;