From 09b0de21f3bbd56768aa5a5b0f43930ff5f995ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:17:24 +0100 Subject: [PATCH 01/88] 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 02/88] 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 03/88] 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 04/88] 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 05/88] 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 06/88] 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 07/88] 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 08/88] 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 09/88] 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 10/88] 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 11/88] 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 12/88] 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 13/88] 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 14/88] 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 15/88] 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 16/88] 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 17/88] 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 18/88] 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 19/88] 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 20/88] 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 21/88] 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 22/88] 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 23/88] 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 24/88] 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 25/88] 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 26/88] 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 27/88] =?UTF-8?q?Switch=20to=20Paul=E2=80=99s=20estimator?= =?UTF-8?q?=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 28/88] 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 29/88] 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 30/88] 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 31/88] 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 32/88] 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 33/88] 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 34/88] 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 35/88] 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 36/88] 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 37/88] 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 38/88] 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 39/88] 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 40/88] 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 41/88] 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 42/88] 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 43/88] 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 44/88] 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 45/88] 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 46/88] 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 47/88] 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 48/88] 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 49/88] 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 50/88] 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 51/88] 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 52/88] 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 53/88] 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 54/88] 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 55/88] 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 56/88] 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 57/88] 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 58/88] 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 59/88] 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 60/88] 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 61/88] 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 62/88] 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 63/88] 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 64/88] 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 65/88] 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 66/88] 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 67/88] 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 68/88] 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 69/88] 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 70/88] 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 71/88] 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 72/88] 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 73/88] 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 74/88] 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 75/88] 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 76/88] 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 77/88] 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 78/88] 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 79/88] 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 80/88] 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 81/88] 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 82/88] 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 83/88] 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 84/88] 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 903012bcff60e3d59f706bec7a920ce3e3aab754 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Mar 2014 14:14:58 +0100 Subject: [PATCH 85/88] 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 86/88] 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 87/88] 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 88/88] 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; } }