From edd16afead95bbf236d974ad895e10cc2ef70033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 15:49:29 +0200 Subject: [PATCH 001/139] Add filter parameters and multicopter defaults to parametrize Pauls estimator correctly when running for multicopters. Estimator itself not updated yet, will be next step. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 6 + .../fw_att_pos_estimator_main.cpp | 44 ++++++ .../fw_att_pos_estimator_params.c | 131 ++++++++++++++++++ 3 files changed, 181 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a9..c1f9db7d1c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -35,6 +35,12 @@ then param set MPC_TILT_MAX 1.0 param set MPC_LAND_SPEED 1.0 param set MPC_LAND_TILT 0.3 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELNE_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + fi set PWM_RATE 400 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 840cd585ec..96db3f20cc 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 @@ -199,6 +199,17 @@ private: int32_t height_delay_ms; int32_t mag_delay_ms; int32_t tas_delay_ms; + float velne_noise; + float veld_noise; + float posne_noise; + float posd_noise; + float mag_noise; + float gyro_pnoise; + float acc_pnoise; + float gbias_pnoise; + float abias_pnoise; + float mage_pnoise; + float magb_pnoise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -207,6 +218,17 @@ private: param_t height_delay_ms; param_t mag_delay_ms; param_t tas_delay_ms; + param_t velne_noise; + param_t veld_noise; + param_t posne_noise; + param_t posd_noise; + param_t mag_noise; + param_t gyro_pnoise; + param_t acc_pnoise; + param_t gbias_pnoise; + param_t abias_pnoise; + param_t mage_pnoise; + param_t magb_pnoise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -302,6 +324,17 @@ FixedwingEstimator::FixedwingEstimator() : _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"); + _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE"); + _parameter_handles.veld_noise = param_find("PE_VELD_NOISE"); + _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE"); + _parameter_handles.posd_noise = param_find("PE_POSD_NOISE"); + _parameter_handles.mag_noise = param_find("PE_MAG_NOISE"); + _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE"); + _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE"); + _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE"); + _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); + _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); + _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); /* fetch initial parameter values */ parameters_update(); @@ -366,6 +399,17 @@ FixedwingEstimator::parameters_update() 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)); + param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise)); + param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise)); + param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise)); + param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise)); + param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise)); + param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise)); + param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise)); + param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise)); + param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); + param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); + param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); return OK; } 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 6138ef39cd..9d01a095c8 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 @@ -115,3 +115,134 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); */ PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); +/** + * Velocity noise in north-east (horizontal) direction. + * + * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); + +/** + * Velocity noise in down (vertical) direction + * + * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); + +/** + * Position noise in north-east (horizontal) direction + * + * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); + +/** + * Position noise in down (vertical) direction + * + * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); + +/** + * Magnetometer measurement noise + * + * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); + +/** + * Gyro process noise + * + * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. + * This noise controls how much the filter trusts the gyro measurements. + * Increasing it makes the filter trust the gyro less and other sensors more. + * + * @min 0.001 + * @max 0.05 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); + +/** + * Accelerometer process noise + * + * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. + * Increasing this value makes the filter trust the accelerometer less + * and other sensors more. + * + * @min 0.05 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); + +/** + * Gyro bias estimate process noise + * + * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. + * Increasing this value will make the gyro bias converge faster but noisier. + * + * @min 0.0000001 + * @max 0.00001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); + +/** + * Accelerometer bias estimate process noise + * + * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Increasing this value makes the bias estimation faster and noisier. + * + * @min 0.0001 + * @max 0.001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); + +/** + * Magnetometer earth frame offsets process noise + * + * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. + * Increasing this value makes the magnetometer earth bias estimate converge + * faster but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); + +/** + * Magnetometer body frame offsets process noise + * + * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. + * Increasing this value makes the magnetometer body bias estimate converge faster + * but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); + From 7b61c927f0420cb3b519972221654176e7c9274b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 16:13:13 +0200 Subject: [PATCH 002/139] Renamed FW filter to EKF to express its generic properties, switched multicopters over to this filter for first tests. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 3 ++- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../estimator.cpp | 0 .../estimator.h | 0 .../fw_att_pos_estimator_main.cpp | 6 +++--- .../fw_att_pos_estimator_params.c | 0 .../module.mk | 2 +- 9 files changed, 9 insertions(+), 8 deletions(-) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.cpp (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.h (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_main.cpp (99%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_params.c (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/module.mk (97%) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec8..9aca3fc5f4 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 # -fw_att_pos_estimator start +ekf_att_pos_estimator start # # Start attitude controller diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index ed3939757a..c75281fcdd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,7 +5,8 @@ # attitude_estimator_ekf start -position_estimator_inav start +ekf_att_pos_estimator start +#position_estimator_inav start mc_att_control start mc_pos_control start diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..1daf8277ea 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,7 +70,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_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 e13421acc5..7f0c59515a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.cpp rename to src/modules/ekf_att_pos_estimator/estimator.cpp diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.h rename to src/modules/ekf_att_pos_estimator/estimator.h diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp similarity index 99% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 96db3f20cc..7857a04693 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -90,7 +90,7 @@ * * @ingroup apps */ -extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); @@ -1255,10 +1255,10 @@ int FixedwingEstimator::trip_nan() { return ret; } -int fw_att_pos_estimator_main(int argc, char *argv[]) +int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c similarity index 100% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk similarity index 97% rename from src/modules/fw_att_pos_estimator/module.mk rename to src/modules/ekf_att_pos_estimator/module.mk index c992959e0d..30955d0ddc 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -35,7 +35,7 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = fw_att_pos_estimator +MODULE_COMMAND = ekf_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ From dca1e7fc611bb44caf1fc586e45105d170955de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 22:40:37 +0200 Subject: [PATCH 003/139] Decomission unmaintained position estimator --- .../att_pos_estimator_ekf/KalmanNav.cpp | 815 ------------------ .../att_pos_estimator_ekf/KalmanNav.hpp | 192 ----- .../att_pos_estimator_ekf/kalman_main.cpp | 157 ---- src/modules/att_pos_estimator_ekf/module.mk | 42 - src/modules/att_pos_estimator_ekf/params.c | 49 -- 5 files changed, 1255 deletions(-) delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp delete mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/module.mk delete mode 100644 src/modules/att_pos_estimator_ekf/params.c diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 668bac5d9c..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,815 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include - -#include "KalmanNav.hpp" -#include -#include - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index caf93bc787..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription _sensors; /**< sensors sub. */ - uORB::Subscription _gps; /**< gps sub. */ - uORB::Subscription _param_update; /**< parameter update sub. */ - // publications - uORB::Publication _pos; /**< position pub. */ - uORB::Publication _localPos; /**< local position pub. */ - uORB::Publication _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d6..0000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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 kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p ]"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d950..0000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edeade..0000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); From ce56d75bc606015728f59a3e811fa48ff9db2979 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 01:37:31 +0200 Subject: [PATCH 004/139] Updated filter to most recent version with accel scale estimation, exposed crucial parameters for cross-vehicle support --- .../ekf_att_pos_estimator/estimator.cpp | 1610 +++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 120 +- .../fw_att_pos_estimator_main.cpp | 25 +- .../fw_att_pos_estimator_params.c | 14 +- 4 files changed, 1188 insertions(+), 581 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index c312173938..ebe30cae0e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,5 @@ #include "estimator.h" - +//#include #include float Vector3f::length(void) const @@ -69,6 +69,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -109,7 +128,6 @@ void swap_var(float &d1, float &d2) AttPosEKF::AttPosEKF() : fusionModeGPS(0), covSkipCount(0), - EAS2TAS(1.0f), statesInitialised(false), fuseVelData(false), fusePosData(false), @@ -120,10 +138,11 @@ AttPosEKF::AttPosEKF() : staticMode(true), useAirspeed(true), useCompass(true), + useRangeFinder(true), numericalProtection(true), storeIndex(0) { - + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -143,8 +162,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float q12; float q13; float q23; - Mat3f Tbn; - Mat3f Tnb; float rotationMag; float qUpdated[4]; float quatMag; @@ -157,7 +174,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; + dVelIMU.z = dVelIMU.z - states[13]; // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -177,8 +194,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - float rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[0] = cosf(0.5f*rotationMag); + float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -260,11 +277,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() void AttPosEKF::CovariancePrediction(float dt) { // scalars - float windVelSigma; - float dAngBiasSigma; - // float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; float daxCov; float dayCov; float dazCov; @@ -284,28 +296,29 @@ void AttPosEKF::CovariancePrediction(float dt) float dax_b; float day_b; float daz_b; + float dvz_b; // arrays - float processNoise[21]; - float SF[14]; + float processNoise[n_states]; + float SF[15]; float SG[8]; float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; + float SPP[8] = {0}; + 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]); + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + // scale gyro bias noise when on ground to allow for faster bias estimation + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -321,29 +334,33 @@ void AttPosEKF::CovariancePrediction(float dt) 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); + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); // 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; + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 2*dvy*q1; SG[0] = q0/2; SG[1] = sq(q3); @@ -366,169 +383,183 @@ void AttPosEKF::CovariancePrediction(float dt) 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]; + SPP[0] = SF[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; - 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[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; + nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; + nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; + nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; + nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; + nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); + nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; + nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; + nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; + nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; + nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; + nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; + nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; + nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); + nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; + nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; + nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; + nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; + nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; + nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; + nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; + nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(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[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(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) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(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) - SPP[5]*(P[7][13] + P[4][13]*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); @@ -543,13 +574,15 @@ void AttPosEKF::CovariancePrediction(float 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[7][21] = P[7][21] + P[4][21]*dt; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(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[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(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) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(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) - SPP[5]*(P[8][13] + P[5][13]*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); @@ -564,13 +597,15 @@ void AttPosEKF::CovariancePrediction(float 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[8][21] = P[8][21] + P[5][21]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(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[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(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) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(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) - SPP[5]*(P[9][13] + P[6][13]*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); @@ -585,13 +620,15 @@ void AttPosEKF::CovariancePrediction(float 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[9][21] = P[9][21] + P[6][21]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; + nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; 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; @@ -606,13 +643,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[10][21] = P[10][21]; + nextP[10][22] = P[10][22]; + nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; + nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; 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; @@ -627,13 +666,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[11][21] = P[11][21]; + nextP[11][22] = P[11][22]; + nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; + nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; 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; @@ -648,13 +689,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[12][21] = P[12][21]; + nextP[12][22] = P[12][22]; + nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; + nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; 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; @@ -669,13 +712,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[13][21] = P[13][21]; + nextP[13][22] = P[13][22]; + nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; + nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; 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; @@ -690,13 +735,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[14][21] = P[14][21]; + nextP[14][22] = P[14][22]; + nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; + nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; 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; @@ -711,13 +758,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[15][21] = P[15][21]; + nextP[15][22] = P[15][22]; + nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; + nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; 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; @@ -732,13 +781,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[16][21] = P[16][21]; + nextP[16][22] = P[16][22]; + nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; + nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; 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; @@ -753,13 +804,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[17][21] = P[17][21]; + nextP[17][22] = P[17][22]; + nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; + nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; 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; @@ -774,13 +827,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[18][21] = P[18][21]; + nextP[18][22] = P[18][22]; + nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; + nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; 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; @@ -795,13 +850,15 @@ void AttPosEKF::CovariancePrediction(float dt) 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[19][21] = P[19][21]; + nextP[19][22] = P[19][22]; + nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; + nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; 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; @@ -816,6 +873,54 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][18] = P[20][18]; nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; + nextP[20][21] = P[20][21]; + nextP[20][22] = P[20][22]; + nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; + nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; + nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + nextP[21][19] = P[21][19]; + nextP[21][20] = P[21][20]; + nextP[21][21] = P[21][21]; + nextP[21][22] = P[21][22]; + nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; + nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; + nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; + nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; + nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; + nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; + nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; + nextP[22][7] = P[22][7] + P[22][4]*dt; + nextP[22][8] = P[22][8] + P[22][5]*dt; + nextP[22][9] = P[22][9] + P[22][6]*dt; + nextP[22][10] = P[22][10]; + nextP[22][11] = P[22][11]; + nextP[22][12] = P[22][12]; + nextP[22][13] = P[22][13]; + nextP[22][14] = P[22][14]; + nextP[22][15] = P[22][15]; + nextP[22][16] = P[22][16]; + nextP[22][17] = P[22][17]; + nextP[22][18] = P[22][18]; + nextP[22][19] = P[22][19]; + nextP[22][20] = P[22][20]; + nextP[22][21] = P[22][21]; + nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -826,16 +931,24 @@ void AttPosEKF::CovariancePrediction(float dt) // setting the coresponding covariance terms to zero. if (onGround || !useCompass) { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); + zeroRows(nextP,16,21); + zeroCols(nextP,16,21); } // If on ground or not using airspeed sensing, inhibit wind velocity // covariance growth. if (onGround || !useAirspeed) { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); } // If the total position variance exceds 1E6 (1000m), then stop covariance @@ -857,7 +970,7 @@ void AttPosEKF::CovariancePrediction(float dt) if (onGround || staticMode) { // copy the portion of the variances we want to // propagate - for (unsigned i = 0; i < 14; i++) { + for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; // force symmetry for observable states @@ -866,7 +979,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t j = 0; j < i; j++) { - if ((i > 12) || (j > 12)) { + if ((i > 13) || (j > 13)) { P[i][j] = 0.0f; } else { P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); @@ -946,12 +1059,12 @@ void AttPosEKF::FuseVelposNED() // Estimate the GPS Velocity, GPS horiz position and height measurement variances. velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); + R_OBS[0] = sq(vneSigma) + 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; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) @@ -1046,11 +1159,11 @@ void AttPosEKF::FuseVelposNED() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) @@ -1080,6 +1193,12 @@ void AttPosEKF::FuseVelposNED() { Kfusion[i] = P[i][stateIndex]*SK; } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) { @@ -1117,24 +1236,38 @@ void AttPosEKF::FuseVelposNED() ForceSymmetry(); ConstrainVariances(); - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } void AttPosEKF::FuseMagnetometer() { - uint8_t obsIndex; - uint8_t indexLimit; + static uint8_t obsIndex; + static float MagPred[3]; + static float SH_MAG[9]; + 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 magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + float R_MAG = sq(magMeasurementSigma); float DCM[3][3] = { {1.0f,0.0f,0.0f} , {0.0f,1.0f,0.0f} , {0.0f,0.0f,1.0f} }; - 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}; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + uint8_t indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1147,46 +1280,30 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } - 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]; q2 = statesAtMagMeasTime[2]; q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; // rotate predicted earth components into body axes and calculate // predicted measurments @@ -1204,7 +1321,7 @@ void AttPosEKF::FuseMagnetometer() 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); + R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; @@ -1217,135 +1334,145 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < n_states; 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.0f; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 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)); + SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][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)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][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)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(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]); + Kfusion[0] = SK_MX[0]*(P[0][19] + 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][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + 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][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + 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][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + 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][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + 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][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + 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][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + 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][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + 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][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + 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][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + 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][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + 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][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + 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][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + 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][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + 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][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + 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][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + 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][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + 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][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + 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][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + 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][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + 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][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + 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][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*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; + fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (unsigned int i=0; i 1e-12f) { - for (uint8_t j= 0; j<=3; j++) + for (uint8_t j= 0; j <= 3; j++) { float quatMagInv = 1.0f/quatMag; states[j] = states[j] * quatMagInv; @@ -1513,38 +1642,38 @@ void AttPosEKF::FuseAirspeed() // 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 i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) + 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++) + for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; 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 j = 16; j <= 22; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) + 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++) + for (uint8_t k = 14; k <= 15; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } } } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1556,6 +1685,343 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +void AttPosEKF::FuseRangeFinder() +{ + + // Local variables + float rngPred; + float SH_RNG[5]; + float H_RNG[23]; + float SK_RNG[6]; + float cosRngTilt; + const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance + + // Copy required states to local variable names + float q0 = statesAtRngTime[0]; + float q1 = statesAtRngTime[1]; + float q2 = statesAtRngTime[2]; + float q3 = statesAtRngTime[3]; + float pd = statesAtRngTime[9]; + float ptd = statesAtRngTime[22]; + + // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data + SH_RNG[4] = sin(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + if (useRangeFinder && cosRngTilt > 0.87f) + { + // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset + // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations + SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_RNG[1] = pd - ptd; + SH_RNG[2] = 1/sq(SH_RNG[0]); + SH_RNG[3] = 1/SH_RNG[0]; + for (uint8_t i = 0; i < n_states; i++) { + H_RNG[i] = 0.0f; + Kfusion[i] = 0.0f; + } + H_RNG[22] = -SH_RNG[3]; + SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); + SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; + SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; + SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; + SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; + SK_RNG[5] = SH_RNG[2]; + Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + + // Calculate the measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // correct the state vector + states[22] = states[22] - Kfusion[22] * innovRng; + + // correct the covariance P = (I - K*H)*P + P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + } + } + +} + +void AttPosEKF::FuseOpticalFlow() +{ + static uint8_t obsIndex; + static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[17]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float R_LOS = 0.01f; + // Transformation matrix from body to navigation axes + Mat3f DCM; + // Transformation matrix from sensor to body axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tsb; + Tsb.x.y = -a3; + Tsb.y.x = a3; + Tsb.x.z = a2; + Tsb.z.x = -a2; + Tsb.y.z = -a1; + Tsb.z.y = a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptData || obsIndex == 1) && !onGround && Tbn.z.z > 0.866f) + { + // Sequential fusion of XY components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptData) + { + // Copy required states to local variable names + q0 = statesAtLosMeasTime[0]; + q1 = statesAtLosMeasTime[1]; + q2 = statesAtLosMeasTime[2]; + q3 = statesAtLosMeasTime[3]; + vn = statesAtLosMeasTime[4]; + ve = statesAtLosMeasTime[5]; + vd = statesAtLosMeasTime[6]; + pd = statesAtLosMeasTime[9]; + ptd = statesAtLosMeasTime[2]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + DCM.x.x = q0 + q1 - q2 - q3; + DCM.y.y = q0 - q1 + q2 - q3; + DCM.z.z = q0 - q1 - q2 + q3; + DCM.x.y = 2*(q12 - q03); + DCM.x.z = 2*(q13 + q02); + DCM.y.x = 2*(q12 + q03); + DCM.y.z = 2*(q23 - q01); + DCM.z.x = 2*(q13 - q02); + DCM.z.y = 2*(q23 + q01); + // calculate transformation from NED to sensor axes + Tns = DCM*Tsb; + Tns = Tns.transpose(); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ((ptd - pd)/Tns.z.z); + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + // divide by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // scale optical flow observation error with total angular rate + R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); + SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); + SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SK_LOS[16] = SH_LOS[0]; + + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + varInnovLOS[0] = 1.0f/SK_LOS[0]; + innovLOS[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + varInnovLOS[1] = 1.0f/SK_LOS[1]; + innovLOS[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1619,51 +2085,54 @@ void AttPosEKF::ResetStoredStates() } // Output the state vector stored at the time that best matches that specified by msec -int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) +int AttPosEKF::RecallStates(float* statesForFusion, 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++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t 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++) { + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { ret++; } } - // } + } return ret; } @@ -1746,7 +2215,7 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); @@ -1783,23 +2252,25 @@ void AttPosEKF::CovarianceInit() 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); - P[4][4] = sq(0.7); + P[4][4] = sq(0.7f); P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); } float AttPosEKF::ConstrainFloat(float val, float min, float max) @@ -1818,45 +2289,52 @@ void AttPosEKF::ConstrainVariances() // 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) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i < 10; i++) { + for (unsigned i = 7; i <= 9; 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)); + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; 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++) { + for (unsigned i = 16; 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++) { + for (unsigned i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } + // Constrain terrain offset variance + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -1870,23 +2348,24 @@ void AttPosEKF::ConstrainStates() // 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) - + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; 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++) { + for (unsigned i = 4; i <= 6; 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++) { + for (unsigned i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -1894,26 +2373,32 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { + for (unsigned i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; 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++) { + for (unsigned i = 16; 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++) { + for (unsigned i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + } void AttPosEKF::ForceSymmetry() @@ -2001,7 +2486,7 @@ void AttPosEKF::ResetVelocity(void) void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { - for (int i = 0; i < n_states; i++) + for (unsigned i = 0; i < n_states; i++) { err->states[i] = states[i]; } @@ -2173,14 +2658,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z + for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities + for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + states[19] = magBias.x; // Magnetic Field Bias X + states[20] = magBias.y; // Magnetic Field Bias Y + states[21] = magBias.z; // Magnetic Field Bias Z + states[22] = 0.0f; // terrain height statesInitialised = true; @@ -2239,10 +2725,10 @@ void AttPosEKF::ZeroVariables() void AttPosEKF::GetFilterState(struct ekf_status_report *state) { - memcpy(state, ¤t_ekf_state, sizeof(state)); + memcpy(state, ¤t_ekf_state, sizeof(*state)); } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { - memcpy(last_error, &last_ekf_error, sizeof(last_error)); + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e62f1a98a5..6effe062d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -45,14 +45,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -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, @@ -82,6 +77,79 @@ public: AttPosEKF(); ~AttPosEKF(); + + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + float a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) + + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + + + // 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 @@ -96,6 +164,8 @@ public: 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 statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtLosMeasTime[n_states]; // filter states at the effective measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -104,6 +174,10 @@ public: 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) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; @@ -115,26 +189,30 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float rngMea; // Ground distance float posNED[3]; // North, East Down position (m) - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output + float innovMag[3]; // innovation output for magnetometer measurements + float varInnovMag[3]; // innovation variance output for magnetometer measurements + float varInnovLOS[2]; // innovation variance output for optical flow measurements Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output + float innovVtas; // innovation output for true airspeed measurements + float innovRng; ///< Range finder innovation for rnge finder measurements + float innovLOS[2]; // Innovations for optical flow LOS rate measurements + float losData[2]; // Optical flow LOS rate measurements float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) + double latRef; // WGS-84 latitude of reference point (rad) + double 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; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS; // ratio f true to equivalent airspeed + unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables float gpsCourse; float gpsVelD; - float gpsLat; - float gpsLon; + double gpsLat; + double gpsLon; float gpsHgt; uint8_t GPSstatus; @@ -148,11 +226,15 @@ public: bool fuseHgtData; // this boolean causes the hgtMea obs to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused + bool fuseRngData; ///< true when range data is fused + bool fuseOptData; // true when optical flow data is fused bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used + bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -172,6 +254,10 @@ void FuseMagnetometer(); void FuseAirspeed(); +void FuseRangeFinder(); + +void FuseOpticalFlow(); + 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); @@ -192,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * time-wise where valid states were updated and invalid remained at the old * value. */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); +int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); @@ -206,7 +292,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7857a04693..8715ba49a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -210,6 +210,7 @@ private: float abias_pnoise; float mage_pnoise; float magb_pnoise; + float eas_noise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,6 +230,7 @@ private: param_t abias_pnoise; param_t mage_pnoise; param_t magb_pnoise; + param_t eas_noise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -335,6 +337,7 @@ FixedwingEstimator::FixedwingEstimator() : _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); + _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); /* fetch initial parameter values */ parameters_update(); @@ -410,6 +413,25 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); + param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); + + if (_ekf) { + // _ekf->yawVarScale = 1.0f; + // _ekf->windVelSigma = 0.1f; + _ekf->dAngBiasSigma = _parameters.gbias_pnoise; + _ekf->dVelBiasSigma = _parameters.abias_pnoise; + _ekf->magEarthSigma = _parameters.mage_pnoise; + _ekf->magBodySigma = _parameters.magb_pnoise; + // _ekf->gndHgtSigma = 0.02f; + _ekf->vneSigma = _parameters.velne_noise; + _ekf->vdSigma = _parameters.veld_noise; + _ekf->posNeSigma = _parameters.posne_noise; + _ekf->posDSigma = _parameters.posd_noise; + _ekf->magMeasurementSigma = _parameters.mag_noise; + _ekf->gyroProcessNoise = _parameters.gyro_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + } return OK; } @@ -473,6 +495,7 @@ FixedwingEstimator::task_main() orb_set_interval(_sensor_combined_sub, 4); #endif + /* sets also parameters in the EKF object */ parameters_update(); /* set initial filter state */ @@ -914,7 +937,7 @@ FixedwingEstimator::task_main() // 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 - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng = _ekf->summedDelAng.zero(); _ekf->summedDelVel = _ekf->summedDelVel.zero(); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index 9d01a095c8..cfcd99858e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -116,7 +116,19 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); /** - * Velocity noise in north-east (horizontal) direction. + * Airspeed measurement noise. + * + * Increasing this value will make the filter trust this sensor + * less and trust other sensors more. + * + * @min 0.5 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f); + +/** + * Velocity measurement noise in north-east (horizontal) direction. * * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 * From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 005/139] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 7cad27a0243a806aa374ffda4ef9e99a854e1c16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 006/139] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..8f39a6330a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -476,6 +478,7 @@ FixedwingEstimator::task_main() _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)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -864,20 +867,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 9c5dbeef3a8c9024d710e95dcce3d877ba357656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:39:43 +0200 Subject: [PATCH 007/139] Proper zero init of the filter --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ebe30cae0e..94fb31457d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2711,6 +2711,8 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + correctedDelAng.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { From b37d0f8f2e3cb0962155113b07a01830c189d4ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:41:34 +0200 Subject: [PATCH 008/139] Safety checks, prepared to use GPS variance --- .../fw_att_pos_estimator_main.cpp | 47 +++++++++++++++---- 1 file changed, 38 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..9d00f1d17b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -319,7 +319,7 @@ FixedwingEstimator::FixedwingEstimator() : _ekf(nullptr) { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -461,6 +461,7 @@ float dt = 0.0f; // time lapsed since last covariance prediction void FixedwingEstimator::task_main() { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); @@ -600,17 +601,23 @@ FixedwingEstimator::task_main() last_run = _gyro.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + } _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; @@ -643,16 +650,22 @@ FixedwingEstimator::task_main() last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f || deltaT < 0.000001f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2])) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; @@ -725,6 +738,21 @@ FixedwingEstimator::task_main() _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + newDataGps = true; } @@ -851,6 +879,7 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + } From 906abbcbb6215e9ae30c51efe3b813a71a963615 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 21:04:05 +0200 Subject: [PATCH 009/139] mavlink: Only write to TX buf if space is available. This is working around a NuttX issue where overflowing the TX buf leads to being unable to send any further data --- src/modules/mavlink/mavlink_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a9f5f4de76..2d71bdce60 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + warnx("TX FAIL"); } } From 1e202a228437c1fa8a04185a5476dcb3d4308759 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:18:13 +0200 Subject: [PATCH 010/139] Updated estimator, not using optical flow for now until proven on the bench --- .../ekf_att_pos_estimator/estimator.cpp | 334 ++---------------- src/modules/ekf_att_pos_estimator/estimator.h | 23 +- 2 files changed, 26 insertions(+), 331 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 94fb31457d..2a7dab890b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -7,13 +7,11 @@ float Vector3f::length(void) const return sqrt(x*x + y*y + z*z); } -Vector3f Vector3f::zero(void) const +void Vector3f::zero(void) { - Vector3f ret = *this; - ret.x = 0.0; - ret.y = 0.0; - ret.z = 0.0; - return ret; + x = 0.0f; + y = 0.0f; + z = 0.0f; } Mat3f::Mat3f() { @@ -69,25 +67,6 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } -// overload * operator to provide a matrix product -Mat3f operator*( Mat3f matIn1, Mat3f matIn2) -{ - Mat3f matOut; - matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; - matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; - matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; - - matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; - matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; - matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; - - matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; - matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; - matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; - - return matOut; -} - // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -1685,6 +1664,19 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +void AttPosEKF::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 0.866f) - { - // Sequential fusion of XY components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseOptData) - { - // Copy required states to local variable names - q0 = statesAtLosMeasTime[0]; - q1 = statesAtLosMeasTime[1]; - q2 = statesAtLosMeasTime[2]; - q3 = statesAtLosMeasTime[3]; - vn = statesAtLosMeasTime[4]; - ve = statesAtLosMeasTime[5]; - vd = statesAtLosMeasTime[6]; - pd = statesAtLosMeasTime[9]; - ptd = statesAtLosMeasTime[2]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - DCM.x.x = q0 + q1 - q2 - q3; - DCM.y.y = q0 - q1 + q2 - q3; - DCM.z.z = q0 - q1 - q2 + q3; - DCM.x.y = 2*(q12 - q03); - DCM.x.z = 2*(q13 + q02); - DCM.y.x = 2*(q12 + q03); - DCM.y.z = 2*(q23 - q01); - DCM.z.x = 2*(q13 - q02); - DCM.z.y = 2*(q23 + q01); - // calculate transformation from NED to sensor axes - Tns = DCM*Tsb; - Tns = Tns.transpose(); - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ((ptd - pd)/Tns.z.z); - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; - // divide by range to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - // scale optical flow observation error with total angular rate - R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - SH_LOS[3] = 1/(pd - ptd); - SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; - SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; - SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; - SH_LOS[7] = 1/sq(pd - ptd); - SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; - SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; - SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; - SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; - SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); - H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); - H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - - // Calculate Kalman gain - SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); - SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); - SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); - SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); - SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - SK_LOS[16] = SH_LOS[0]; - - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - varInnovLOS[0] = 1.0f/SK_LOS[0]; - innovLOS[0] = losPred[0] - losData[0]; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseOptData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); - H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); - H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - - // Calculate Kalman gains - Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - varInnovLOS[1] = 1.0f/SK_LOS[1]; - innovLOS[1] = losPred[1] - losData[1]; - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovLOS[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 < n_states; i++) - { - for (uint8_t j = 0; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j <= 8; j++) - { - KH[i][j] = 0.0f; - } - KH[i][9] = Kfusion[i] * H_LOS[9]; - for (uint8_t j = 10; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - KH[i][22] = Kfusion[i] * H_LOS[22]; - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; - KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; - } - } - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::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 Date: Mon, 21 Apr 2014 01:18:34 +0200 Subject: [PATCH 011/139] ekf_att_pos_estimator: Using right app name --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0dafc4311e..59ce45e16a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1211,7 +1211,7 @@ FixedwingEstimator::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 6000, From c08544721a0ac4293786ce410dcd0084e1f7cfe6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:20:35 +0200 Subject: [PATCH 012/139] att_pos_estimator_ekf: Update filter to new filter API --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 59ce45e16a..8287757196 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -978,8 +978,8 @@ FixedwingEstimator::task_main() // or the time limit will be exceeded at the next IMU update if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); - _ekf->summedDelAng = _ekf->summedDelAng.zero(); - _ekf->summedDelVel = _ekf->summedDelVel.zero(); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); dt = 0.0f; } From aa3aafb1e5d060593529af72bcf22d6351374df9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 10:47:15 +0200 Subject: [PATCH 013/139] Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset --- .../ekf_att_pos_estimator/estimator.cpp | 139 +++++++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 19 ++- 2 files changed, 123 insertions(+), 35 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 2a7dab890b..18e17d4d9f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,7 +1,37 @@ #include "estimator.h" -//#include #include +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +//#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +static void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +static void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -15,6 +45,10 @@ void Vector3f::zero(void) } Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { x.x = 1.0f; x.y = 0.0f; x.z = 0.0f; @@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() : storeIndex(0) { InitialiseParameters(); + ZeroVariables(); } AttPosEKF::~AttPosEKF() @@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED() void AttPosEKF::FuseMagnetometer() { - static uint8_t obsIndex; - static float MagPred[3]; - static float SH_MAG[9]; - 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 magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - float R_MAG = sq(magMeasurementSigma); - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; @@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer() // 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; + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; // scale magnetometer observation error with total angular rate R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); @@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { bool err = false; + // check all integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + err = true; + } // delta velocities + // 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; + ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; + ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; + ekf_debug("P NaN"); err = true; } // covariance matrix } @@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); err = true; } // Kalman gains if (!isfinite(states[i])) { err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; } // state matrix } @@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED); @@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(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; - + magstate.q3 = 1.0f; + magstate.magN = 0.4f; + magstate.magE = 0.0f; + magstate.magD = 0.3f; + magstate.magXbias = 0.0f; + magstate.magYbias = 0.0f; + magstate.magZbias = 0.0f; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM.identity(); // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables() } correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables() statetimeStamp[i] = 0; } + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 3a8f45d9af..734ad0c05d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -33,6 +33,7 @@ public: Mat3f(); + void identity(); Mat3f transpose(void) const; }; @@ -140,7 +141,23 @@ public: accelProcessNoise = 0.5f; } - + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; // Global variables From 595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 11:02:27 +0200 Subject: [PATCH 014/139] ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence --- .../ekf_att_pos_estimator/estimator.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 18e17d4d9f..95afbc181d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(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; - magstate.q3 = 1.0f; - magstate.magN = 0.4f; - magstate.magE = 0.0f; - magstate.magD = 0.3f; - magstate.magXbias = 0.0f; - magstate.magYbias = 0.0f; - magstate.magZbias = 0.0f; + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = magData.x; + magstate.magE = magData.y; + magstate.magD = magData.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM.identity(); + magstate.DCM = DCM; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions From 8ae50a4ba524f840dc7d96b79c5dbad1b7be15f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 015/139] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 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 8ea6118022..e9c036e858 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 @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -410,6 +412,7 @@ FixedwingEstimator::task_main() _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)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -797,20 +800,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 016/139] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 706d08055d2bf63d16c72e86cd60f0e33a20bc4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:18:30 +0200 Subject: [PATCH 017/139] Better / cleaner initialization of the attitude estimator --- .../ekf_att_pos_estimator/estimator.cpp | 24 ++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 34 ++++++++++++------- 3 files changed, 38 insertions(+), 22 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 95afbc181d..5f4bf07293 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -4,7 +4,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -//#define EKF_DEBUG +#define EKF_DEBUG #ifdef EKF_DEBUG #include @@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KH NaN"); + } + if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KHP NaN"); + } + if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; - ekf_debug("P NaN"); err = true; } // covariance matrix + if (err) { + ekf_debug("P NaN"); + } } if (!isfinite(Kfusion[i])) { @@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) { //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 734ad0c05d..5e60e506f6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -308,7 +308,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3]); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); float ConstrainFloat(float val, float min, float max); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8287757196..c8005f3d3f 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -606,7 +606,6 @@ FixedwingEstimator::task_main() /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } @@ -650,14 +649,14 @@ FixedwingEstimator::task_main() IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; - last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } + last_run = _sensor_combined.timestamp; + // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; @@ -823,6 +822,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -830,27 +831,34 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + const char* str = "switching to dynamic state"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -912,7 +920,7 @@ FixedwingEstimator::task_main() double lon = home.lon; float alt = home.alt; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = home.lat * 1e7; @@ -942,7 +950,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); } } From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: [PATCH 018/139] commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f2..077184bfbd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 904ada124baea8ef744535053a0c3b40871565e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:15:33 +0200 Subject: [PATCH 019/139] ekf: Put reset statements after variable zero operation to ensure values get initialized correctly --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5f4bf07293..99de161aac 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; @@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) states[21] = magBias.z; // Magnetic Field Bias Z states[22] = 0.0f; // terrain height + ResetVelocity(); + ResetPosition(); + ResetHeight(); + statesInitialised = true; // initialise the covariance matrix From 39a0d4e54db1252678eb8f4ebc872c589da2d9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:24:29 +0200 Subject: [PATCH 020/139] Better error handling / reporting in filter --- .../ekf_att_pos_estimator/estimator.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 99de161aac..14761831cf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2243,18 +2243,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); err = true; + goto out; } // delta velocities // check all states and covariance matrices @@ -2264,28 +2267,24 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KH NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KHP NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; err = true; - } // covariance matrix - if (err) { ekf_debug("P NaN"); - } + } // covariance matrix } if (!isfinite(Kfusion[i])) { @@ -2293,6 +2292,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; + goto out; } // Kalman gains if (!isfinite(states[i])) { @@ -2300,9 +2300,11 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; + goto out; } // state matrix } +out: if (err) { FillErrorReport(err_report); } From 125f0b2f88c390bfde92ebe5423a0913e0e1b114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:25:38 +0200 Subject: [PATCH 021/139] Added trap to filter to catch NaN handling --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8005f3d3f..ac34af48a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -858,6 +858,11 @@ FixedwingEstimator::task_main() } } + // XXX trap for testing + if (check == 1 || check == 2) { + errx(1, "NUMERIC ERROR IN FILTER"); + } + // If non-zero, we got a filter reset if (check) { From 5b7209639d42290dc5b1e3fc53fc81a455d75b51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 09:56:38 +0200 Subject: [PATCH 022/139] Revert "HIL: Increased MAVLink link wait time based on previous experience that this is timing sensitive." This reverts commit 78bf7ed969624eacea35ae2bf402596aecb3c5a6. --- ROMFS/px4fmu_common/init.d/rcS | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ccac0a2631..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -403,8 +403,7 @@ then then if [ $HIL == yes ] then - # Sleep required to ensure the link is up - sleep 5 + sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From bd637697e4880d3efbb79c2c05647564d3582cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:26 +0200 Subject: [PATCH 023/139] Removed verbose print --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ac34af48a4..ad7cb3687a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -920,6 +920,7 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); + warnx("HOME SET"); double lat = home.lat; double lon = home.lon; From 1e80e624916a0eb1b13adccb4f700adeeee66bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:44 +0200 Subject: [PATCH 024/139] ekf: Better variable zeroing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..ac9abf5ca3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,8 +153,16 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), - storeIndex(0) + storeIndex(0), + gpsHgt(0.0f), + baroHgt(0.0f), + GPSstatus(0), + VtasMeas(0.0f), { + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + InitialiseParameters(); ZeroVariables(); } @@ -1967,7 +1975,7 @@ void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, flo void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { staticMode = !(GPSstatus > GPS_FIX_2D); } @@ -2515,6 +2523,7 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + } void AttPosEKF::GetFilterState(struct ekf_status_report *state) From 4585df1182083c39f2439bb7b88953dcc3575240 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:31 +0200 Subject: [PATCH 025/139] Robustified filter init / sequencing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 6 ++++-- src/modules/ekf_att_pos_estimator/estimator.h | 1 + .../fw_att_pos_estimator_main.cpp | 13 +++++++++---- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ac9abf5ca3..5de22fdaec 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), + refSet(false), storeIndex(0), gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f), + VtasMeas(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } } @@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; + refSet = true; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 5e60e506f6..e118ae5734 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -211,6 +211,7 @@ public: double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad7cb3687a..19111a9b42 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -192,6 +192,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -720,6 +721,9 @@ FixedwingEstimator::task_main() } else { + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); @@ -859,7 +863,7 @@ FixedwingEstimator::task_main() } // XXX trap for testing - if (check == 1 || check == 2) { + if (check == 1) { errx(1, "NUMERIC ERROR IN FILTER"); } @@ -907,7 +911,7 @@ FixedwingEstimator::task_main() // XXX we rather want to check all updated - if (hrt_elapsed_time(&start_time) > 100000) { + if (hrt_elapsed_time(&_gps_start_time) > 50000) { bool home_set; orb_check(_home_sub, &home_set); @@ -920,7 +924,6 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); - warnx("HOME SET"); double lat = home.lat; double lon = home.lon; @@ -942,7 +945,9 @@ FixedwingEstimator::task_main() // XXX this is not multithreading safe map_projection_init(lat, lon); - mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); + warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); _gps_initialized = true; From a30411e9f2438018a08c0965261067940f88be10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:53 +0200 Subject: [PATCH 026/139] Fixed printing in attitude control --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2f84dc963b..5ded7aa76f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -771,7 +771,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -780,16 +780,16 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } /* From d4785d4b6561e1f381abd0721e1701f4c141e2fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:13:50 +0200 Subject: [PATCH 027/139] Use INAV as default to not break existing setups --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index c75281fcdd..268eb9bba0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,8 +5,8 @@ # attitude_estimator_ekf start -ekf_att_pos_estimator start -#position_estimator_inav start +#ekf_att_pos_estimator start +position_estimator_inav start mc_att_control start mc_pos_control start From ad1be765bf36a6068fda25e3c62c92e275aebc6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:14:23 +0200 Subject: [PATCH 028/139] Fix warnings, use more efficient atan2f where it can be safely used --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5de22fdaec..6eceb21f8e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2250,21 +2250,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { err_report->statesNaN = true; - ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; - ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; - ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; } // delta velocities @@ -2381,8 +2381,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f float magX, magY; float initialHdg, cosHeading, sinHeading; - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); cosRoll = cosf(initialRoll); sinRoll = sinf(initialRoll); From 9358eb2428275f78ad5b1e06b42840927092ac89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:22:03 +0200 Subject: [PATCH 029/139] Fixed string formatting error --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5315c5e387..e260aae45c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -655,7 +655,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { From 0d50b3ea866a9ca0b9271b8c861f1d9e2a61a24a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:06:25 +0200 Subject: [PATCH 030/139] Fix struct inits --- .../fw_att_pos_estimator_main.cpp | 67 +++++++++++++++---- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad8c1b4c00..1c943137a1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -103,8 +103,6 @@ uint32_t millis() return IMUmsec; } -static void print_status(); - class FixedwingEstimator { public: @@ -305,6 +303,25 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), _estimator_status_pub(-1), + _att({}), + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + #ifdef SENSOR_COMBINED_SUB + _sensor_combined({}), + #endif + _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -356,6 +373,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); close(fd); + + if (res) { + warnx("G SCALE FAIL"); + } } fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -363,6 +384,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); close(fd); + + if (res) { + warnx("A SCALE FAIL"); + } } fd = open(MAG_DEVICE_PATH, O_RDONLY); @@ -370,6 +395,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); close(fd); + + if (res) { + warnx("M SCALE FAIL"); + } } } @@ -538,12 +567,19 @@ FixedwingEstimator::task_main() fds[1].events = POLLIN; #endif - hrt_abstime start_time = hrt_absolute_time(); - bool newDataGps = false; bool newAdsData = false; bool newDataMag = false; + // Reset relevant structs + _gps = {}; + + #ifndef SENSOR_COMBINED_SUB + _gyro = {}; + #else + _sensor_combined = {}; + #endif + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -624,9 +660,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _gyro.z; } - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; @@ -672,9 +710,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; } - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + if (accel_updated) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; @@ -885,7 +925,7 @@ FixedwingEstimator::task_main() rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; // Copy all states or at least all that we can fit - int i = 0; + unsigned 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; @@ -1173,6 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { @@ -1185,6 +1227,7 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); + _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1259,7 +1302,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); From 2d978beefbc010d962034a0f0d588cbf46a063f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:16:44 +0200 Subject: [PATCH 031/139] Compile fixes --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 6eceb21f8e..ffebcd4776 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2307,7 +2307,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, states[i]); + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; } // state matrix diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1c943137a1..fdb5dd5883 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1213,8 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; } if (_local_pos.v_xy_valid) { @@ -1227,7 +1227,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; From 18a932fe948b06b87d65f5d13102274e5683fab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:35:22 +0200 Subject: [PATCH 032/139] Better fake / simulation values --- src/drivers/gps/gps.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..01be80dcec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -276,14 +276,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: [PATCH 033/139] Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..148a0aafa2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 925430796ed56e97bf2310423361a8e6e29e350b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:37:26 +0200 Subject: [PATCH 034/139] Reworked how we deal with altitudes --- .../fw_att_pos_estimator_main.cpp | 79 ++++++++++++------- .../fw_att_pos_estimator_params.c | 11 +++ 2 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fdb5dd5883..a02b2c34fb 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -191,6 +191,7 @@ private: perf_counter_t _perf_airspeed; ///yawVarScale = 1.0f; @@ -568,18 +574,10 @@ FixedwingEstimator::task_main() #endif bool newDataGps = false; + bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; - // Reset relevant structs - _gps = {}; - - #ifndef SENSOR_COMBINED_SUB - _gyro = {}; - #else - _sensor_combined = {}; - #endif - while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -611,6 +609,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -618,6 +617,12 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /* Reset baro reference if switching to HIL */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + _baro_init = false; + _gps_initialized = false; + } + /** * PART ONE: COLLECT ALL DATA **/ @@ -813,10 +818,17 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _ekf->baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude; - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; } #ifndef SENSOR_COMBINED_SUB @@ -904,9 +916,9 @@ FixedwingEstimator::task_main() } } - // XXX trap for testing + // warn on fatal resets if (check == 1) { - errx(1, "NUMERIC ERROR IN FILTER"); + warnx("NUMERIC ERROR IN FILTER"); } // If non-zero, we got a filter reset @@ -960,7 +972,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -968,26 +980,37 @@ FixedwingEstimator::task_main() // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; - float alt = _gps.alt / 1e3f; + float gps_alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = gps_alt - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.alt; - _local_pos.ref_alt = alt; + _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; - // Store - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref = _baro.altitude; - _ekf->baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); - warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); _gps_initialized = true; @@ -1089,9 +1112,9 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newAdsData && _ekf->statesInitialised) { + if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index cfcd99858e..d2c6e1f15e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -258,3 +258,14 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); From 6ab878c0218f26e5fa71053b75d7075b594b937e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:38:37 +0200 Subject: [PATCH 035/139] Emit the local position against the GPS reference - this means it can jump. --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a02b2c34fb..a82e8d7044 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1203,7 +1203,7 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9]; + _local_pos.z = _ekf->states[9] + _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; From 0745334ac46c40ed407dc11ebbb7c252e37bcb43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 11:41:40 +0200 Subject: [PATCH 036/139] Reset init flags as well --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..6eccc4c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,6 +2407,19 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // Clear the init flag statesInitialised = false; + // Clear other flags, waiting for new data + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + // onGround(true), + // staticMode(true), + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: [PATCH 037/139] Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/estimator.cpp | 33 +++++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 7 ++-- .../fw_att_pos_estimator_main.cpp | 13 +++++--- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 8ac2d61541..1ca34ec307 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() : gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f) + VtasMeas(0.0f), + magDeclination(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED); + InitializeDynamic(velNED, magDeclination); return 1; } @@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound() return 0; } -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f magY = my * cosRoll - mz * sinRoll; initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg -= declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; } -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { // Clear the init flag @@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); + magDeclination = declination; + // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states @@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) magstate.q1 = initQuat[1]; magstate.q2 = initQuat[2]; magstate.q3 = initQuat[3]; - magstate.magN = magData.x; - magstate.magE = magData.y; - magstate.magD = magData.z; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; magstate.magXbias = magBias.x; magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; @@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { //store initial lat,long and height latRef = referenceLat; @@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED); + InitializeDynamic(initvelNED, declination); } void AttPosEKF::ZeroVariables() diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e118ae5734..378107b698 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -208,6 +208,7 @@ public: float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -309,7 +310,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); float ConstrainFloat(float val, float min, float max); @@ -334,7 +335,7 @@ 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]); +void InitializeDynamic(float (&initvelNED)[3], float declination); protected: @@ -342,7 +343,7 @@ bool FilterHealthy(); void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d7044..23f9e80bd8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: [PATCH 038/139] ekf_att_pos_estimator: local position reference fixed --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd8..fb019a354e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; From 848c1c2552996281685d2691c1b394f25a4391b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 21:42:58 +0200 Subject: [PATCH 039/139] ekf: More complete re-initialization --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1ca34ec307..e1ffc15d3b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2536,6 +2536,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + magBias.zero(); + magState.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From d54f9973f26e5503f33ad53fd927696089a742ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:13:42 +0200 Subject: [PATCH 040/139] Fixed typos in declination table lookup --- src/lib/geo/geo_mag_declination.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f877214..cfee6b423d 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -115,8 +115,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); From e2af77123fb14e9e19d1aa9e02f04b62a342c724 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:25:36 +0200 Subject: [PATCH 041/139] ekf: Cleanup init --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index e1ffc15d3b..a294210459 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2455,7 +2455,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) Mat3f DCM; quat2Tbn(DCM, initQuat); Vector3f initMagNED; - initMagXYZ = magData - magBias; initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; 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; @@ -2497,13 +2496,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -2537,7 +2529,7 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); magBias.zero(); - magState.zero(); + magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From be1f39e3851036e43be1bea0fa09096691559a0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:52:37 +0200 Subject: [PATCH 042/139] ekf Print declination on init --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fb019a354e..b87338282a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1013,7 +1013,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; From 7a4049b12a8be77bb67906c666cddf93286d39fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 16:55:08 +0200 Subject: [PATCH 043/139] Fix use of declination in estimator, remove bogus measurement value reset on reinit --- src/modules/ekf_att_pos_estimator/estimator.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a294210459..86b7efafb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2396,7 +2396,7 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initialHdg = atan2f(-magY, magX); /* true heading is the mag heading minus declination */ - initialHdg -= declination; + initialHdg += declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2528,8 +2528,6 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - magBias.zero(); - magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From 6cb96d074d282af92540b6481d843c2295426773 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 17:01:32 +0200 Subject: [PATCH 044/139] EKF: Introduce proper check flags for sensor init states --- .../fw_att_pos_estimator_main.cpp | 40 ++++++++++++++++--- 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index b87338282a..40729e94b1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -194,6 +194,9 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; int _mavlink_fd; @@ -341,6 +344,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _baro_init(false), _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), _mavlink_fd(-1), _ekf(nullptr) { @@ -617,10 +623,30 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); - /* Reset baro reference if switching to HIL */ + /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, we got plenty of time - make sure real sensors are off */ + usleep(250000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + _baro_init = false; _gps_initialized = false; + + /* now skip this loop and get data on the next one */ + continue; } /** @@ -663,12 +689,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; @@ -713,12 +741,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; @@ -837,6 +867,8 @@ FixedwingEstimator::task_main() if (mag_updated) { + _mag_valid = true; + perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB @@ -961,11 +993,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - // Wait long enough to ensure all sensors updated once - // XXX we rather want to check all updated - - - if (hrt_elapsed_time(&_gps_start_time) > 50000) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // bool home_set; // orb_check(_home_sub, &home_set); From 319ce3de1005d7199a992b227c7e75acb9db376b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:40:23 +0200 Subject: [PATCH 045/139] Minor cleanups in EKF estimator --- .../fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 40729e94b1..e563b4b87c 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -625,8 +625,8 @@ FixedwingEstimator::task_main() /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { - /* system is in HIL now, we got plenty of time - make sure real sensors are off */ - usleep(250000); + /* system is in HIL now, wait for measurements to come in one last round */ + usleep(65000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -1399,13 +1399,13 @@ int FixedwingEstimator::trip_nan() { _ekf->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 #1 with NaN values"); + _ekf->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 #2 with NaN values"); + _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates + usleep(100000); warnx("tripping covariance #3 with NaN values"); _ekf->P[3][3] = nan_val; // covariance matrix From 21edf72779286bbfb3c4c6bb4acad5c353300024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:15:11 +0200 Subject: [PATCH 046/139] Do not send a critical message when switching to dynamic state --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e563b4b87c..05cc39f11b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -936,7 +936,7 @@ FixedwingEstimator::task_main() { const char* str = "switching to dynamic state"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } From 23937150bce38463612ac170803a06a3424d480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 18:07:57 +0200 Subject: [PATCH 047/139] Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results. --- .../ekf_att_pos_estimator/estimator.cpp | 85 ++++++++-------- .../fw_att_pos_estimator_main.cpp | 96 ++++++++++++------- 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 86b7efafb3..a6d6a9db55 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -138,35 +138,15 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - useRangeFinder(true), - numericalProtection(true), - refSet(false), - storeIndex(0), - gpsHgt(0.0f), - baroHgt(0.0f), - GPSstatus(0), - VtasMeas(0.0f), - magDeclination(0.0f) -{ - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ +{ - InitialiseParameters(); ZeroVariables(); + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -2348,7 +2328,7 @@ int AttPosEKF::CheckAndBound() } // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { + if (dtIMU > 0.3f) { ResetVelocity(); ResetPosition(); @@ -2424,24 +2404,10 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { - // Clear the init flag - statesInitialised = false; - - // Clear other flags, waiting for new data - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - // onGround(true), - // staticMode(true), - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - - ZeroVariables(); - + // Fill variables with valid data + velNED[0] = initvelNED[0]; + velNED[1] = initvelNED[1]; + velNED[2] = initvelNED[2]; magDeclination = declination; // Calculate initial filter quaternion states from raw measurements @@ -2513,6 +2479,30 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do void AttPosEKF::ZeroVariables() { + + // Initialize on-init initialized variables + fusionModeGPS = 0; + covSkipCount = 0; + statesInitialised = false; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + // Do the data structure init for (unsigned i = 0; i < n_states; i++) { for (unsigned j = 0; j < n_states; j++) { @@ -2529,6 +2519,9 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 05cc39f11b..07dfdbd682 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -546,24 +546,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - /* set initial filter state */ - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - _ekf->fuseHgtData = false; - _ekf->fuseMagData = false; - _ekf->fuseVtasData = false; - _ekf->statesInitialised = false; - - /* initialize measurement data */ - _ekf->VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; - _ekf->dVelIMU.x = 0.0f; - _ekf->dVelIMU.y = 0.0f; - _ekf->dVelIMU.z = 0.0f; - _ekf->dAngIMU.x = 0.0f; - _ekf->dAngIMU.y = 0.0f; - _ekf->dAngIMU.z = 0.0f; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; /* wakeup source(s) */ struct pollfd fds[2]; @@ -621,12 +605,14 @@ FixedwingEstimator::task_main() bool accel_updated; bool mag_updated; + hrt_abstime last_sensor_timestamp; + perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ - usleep(65000); + usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -644,8 +630,13 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; - /* now skip this loop and get data on the next one */ + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } @@ -653,8 +644,6 @@ FixedwingEstimator::task_main() * PART ONE: COLLECT ALL DATA **/ - hrt_abstime last_sensor_timestamp; - /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -668,7 +657,7 @@ FixedwingEstimator::task_main() } last_sensor_timestamp = _gyro.timestamp; - _ekf.IMUmsec = _gyro.timestamp / 1e3f; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -689,6 +678,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -696,6 +690,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -741,6 +740,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -748,6 +752,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -986,6 +995,22 @@ FixedwingEstimator::task_main() _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; + + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + // Let the system re-initialize itself + continue; + } @@ -995,15 +1020,13 @@ FixedwingEstimator::task_main() if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - // bool home_set; - // orb_check(_home_sub, &home_set); - // struct home_position_s home; - // orb_copy(ORB_ID(home_position), _home_sub, &home); + float initVelNED[3]; if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; + + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; @@ -1018,9 +1041,6 @@ FixedwingEstimator::task_main() // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; @@ -1029,7 +1049,7 @@ FixedwingEstimator::task_main() // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; @@ -1041,21 +1061,23 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; } else if (!_ekf->statesInitialised) { - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; + + initVelNED[0] = 0.0f; + initVelNED[1] = 0.0f; + initVelNED[2] = 0.0f; _ekf->posNED[0] = 0.0f; _ekf->posNED[1] = 0.0f; _ekf->posNED[2] = 0.0f; _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } } From da67b2b241ca8eff0d25bfe10b0213f3d570dd50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 May 2014 17:17:25 +0200 Subject: [PATCH 048/139] Disable time compensation for further testing --- .../ekf_att_pos_estimator/estimator.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a6d6a9db55..5a6f879b8c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // Work around a GCC compiler bug - we know 64bit support on ARM is - // sketchy in GCC. - uint64_t timeDelta; + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // Work around a GCC compiler bug - we know 64bit support on ARM is + // // sketchy in GCC. + // uint64_t timeDelta; - if (msec > statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; - } else { - timeDelta = statetimeStamp[storeIndex] - msec; - } + // if (msec > statetimeStamp[storeIndex]) { + // timeDelta = msec - statetimeStamp[storeIndex]; + // } else { + // timeDelta = statetimeStamp[storeIndex] - msec; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (unsigned 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 + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { From e098591a58771c4f49776a7b061939055c7cfc1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 13:57:32 +0200 Subject: [PATCH 049/139] Addressed linter concerns --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 4aba65b4aa..ba8a58f650 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -302,6 +302,8 @@ FixedwingEstimator::FixedwingEstimator() : _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _mission_sub(-1), + _home_sub(-1), /* publications */ _att_pub(-1), @@ -712,6 +714,8 @@ FixedwingEstimator::task_main() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; + } else { + accel_updated = false; } last_accel = _sensor_combined.accelerometer_timestamp; @@ -802,7 +806,6 @@ FixedwingEstimator::task_main() perf_count(_perf_gps); if (_gps.fix_type < 3) { - gps_updated = false; newDataGps = false; } else { From 5f392c80ad9bbc0e1f424125b70f277c2c677be1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 14:42:33 +0200 Subject: [PATCH 050/139] More debug in filter --- .../ekf_att_pos_estimator/estimator.cpp | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5a6f879b8c..068d665a57 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1268,7 +1268,7 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t i = 0; i < n_states; i++) { H_MAG[i] = 0.0f; } - uint8_t indexLimit; + unsigned indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1281,11 +1281,11 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 22; + indexLimit = n_states; } else { - indexLimit = 13; + indexLimit = 13 + 1; } // Sequential fusion of XYZ components to spread processing load across @@ -1483,7 +1483,7 @@ void AttPosEKF::FuseMagnetometer() if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) { // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) + for (uint8_t j= 0; j < indexLimit; j++) { states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; } @@ -1500,7 +1500,7 @@ void AttPosEKF::FuseMagnetometer() // 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 i = 0; i < indexLimit; i++) { for (uint8_t j = 0; j <= 3; j++) { @@ -1522,9 +1522,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i<=indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j<=indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { KHP[i][j] = 0.0f; for (uint8_t k = 0; k <= 3; k++) @@ -1541,9 +1541,9 @@ void AttPosEKF::FuseMagnetometer() } } } - for (uint8_t i = 0; i <= indexLimit; i++) + for (uint8_t i = 0; i < indexLimit; i++) { - for (uint8_t j = 0; j <= indexLimit; j++) + for (uint8_t j = 0; j < indexLimit; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -2001,7 +2001,22 @@ void AttPosEKF::CovarianceInit() float AttPosEKF::ConstrainFloat(float val, float min, float max) { - return (val > max) ? max : ((val < min) ? min : val); + float ret; + if (val > max) { + ret = max; + ekf_debug("> max: %8.4f, val: %8.4f", min); + } else if (val < min) { + ret = min; + ekf_debug("< min: %8.4f, val: %8.4f", max); + } else { + ret = val; + } + + if (!isfinite) { + ekf_debug("constrain: non-finite!"); + } + + return ret; } void AttPosEKF::ConstrainVariances() From 7ec8fe8d61ea56b632d8b02bae5d847a3f031771 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:42:46 +0200 Subject: [PATCH 051/139] Experimental init delay --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ba8a58f650..d96ae56373 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1021,7 +1021,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (hrt_absolute_time() > 2 * 1000 * 1000 && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; From 5581802f0f2f4bb34ac8c43b8aa9c1c555013758 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:45:55 +0200 Subject: [PATCH 052/139] ekf: Move dt inside class --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index d96ae56373..997ea17598 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -504,14 +504,13 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } -float dt = 0.0f; // time lapsed since last covariance prediction - void FixedwingEstimator::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); + float dt = 0.0f; // time lapsed since last covariance prediction if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); From eeba000b8728e9b7a8daae0c0ad129257ec39403 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 19:29:43 +0200 Subject: [PATCH 053/139] stupid fix --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 +++++--- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 068d665a57..4239db6691 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2004,15 +2004,15 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", min); + ekf_debug("> max: %8.4f, val: %8.4f", max, val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", max); + ekf_debug("< min: %8.4f, val: %8.4f", min, val); } else { ret = val; } - if (!isfinite) { + if (!isfinite(val)) { ekf_debug("constrain: non-finite!"); } @@ -2119,7 +2119,9 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias + ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 997ea17598..9065063ce1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1379,7 +1379,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); From 077de5eb0b0093e131f94f063a107f290d6c293b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:16:04 +0200 Subject: [PATCH 054/139] Clean implementation of filter startup delay implementation --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9065063ce1..0921de8690 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -97,6 +97,7 @@ __EXPORT uint32_t millis(); static uint64_t last_run = 0; static uint64_t IMUmsec = 0; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; uint32_t millis() { @@ -194,6 +195,7 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + uint64_t _filter_start_time; bool _gyro_valid; bool _accel_valid; bool _mag_valid; @@ -511,6 +513,7 @@ FixedwingEstimator::task_main() _ekf = new AttPosEKF(); float dt = 0.0f; // time lapsed since last covariance prediction + _filter_start_time = hrt_absolute_time(); if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); @@ -636,6 +639,7 @@ FixedwingEstimator::task_main() _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; + _filter_start_time = last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; @@ -1020,7 +1024,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_absolute_time() > 2 * 1000 * 1000 && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; From 1b3007aa813829e401849f53552ac5917da71f5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:18:09 +0200 Subject: [PATCH 055/139] Re-enabled time compensation --- .../ekf_att_pos_estimator/estimator.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 4239db6691..9d60f5c0e5 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1815,41 +1815,41 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // Work around a GCC compiler bug - we know 64bit support on ARM is - // // sketchy in GCC. - // uint64_t timeDelta; + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t timeDelta; - // if (msec > statetimeStamp[storeIndex]) { - // timeDelta = msec - statetimeStamp[storeIndex]; - // } else { - // timeDelta = statetimeStamp[storeIndex] - msec; - // } + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (unsigned 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 + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned 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 (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { From f975f9837e03cdddad20f934dfce113a6172c646 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 20:48:21 +0200 Subject: [PATCH 056/139] ekf: less console spam --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e5..ada5a541bf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2119,9 +2119,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { From 6906dc4edac85113c3217b250c84e89ed1fe26a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 May 2014 11:23:48 +0200 Subject: [PATCH 057/139] Minor improvements to estimator --- src/modules/ekf_att_pos_estimator/estimator.cpp | 11 ++++++++--- src/modules/ekf_att_pos_estimator/estimator.h | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e5..d05506396f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,6 @@ #include "estimator.h" #include +#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -2119,9 +2120,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { @@ -2483,12 +2482,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { - //store initial lat,long and height + // store initial lat,long and height latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; refSet = true; + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); InitializeDynamic(initvelNED, declination); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 378107b698..e821089f23 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -198,6 +198,7 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float rngMea; // Ground distance float posNED[3]; // North, East Down position (m) From ec409a1337a0f9c73cbe54776b254464e3dcd1d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 08:53:09 +0200 Subject: [PATCH 058/139] EKF / Paul Riseborough: Added guards for mag and airspeed innovations --- .../ekf_att_pos_estimator/estimator.cpp | 45 +++++++++++++++++-- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index d05506396f..23ecd89ac3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1347,7 +1347,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[19] = 1.0f; // Calculate Kalman gain - SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][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)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][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)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][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)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][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)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MX[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[19][19] += 0.1f*R_MAG; + obsIndex = 1; + return; + } 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; @@ -1399,7 +1408,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[20] = 1; // Calculate Kalman gain - SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][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[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][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[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MY[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[20][20] += 0.1f*R_MAG; + obsIndex = 2; + return; + } 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; @@ -1445,7 +1463,16 @@ void AttPosEKF::FuseMagnetometer() H_MAG[21] = 1; // Calculate Kalman gain - SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][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)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][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[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][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)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][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[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MZ[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[21][21] += 0.1f*R_MAG; + obsIndex = 3; + return; + } 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; @@ -1565,6 +1592,7 @@ void AttPosEKF::FuseAirspeed() float vwe; float R_TAS = sq(airspeedMeasurementSigma); float SH_TAS[3]; + float SK_TAS; float VtasPred; // Copy required states to local variable names @@ -1594,7 +1622,16 @@ void AttPosEKF::FuseAirspeed() H_TAS[15] = -SH_TAS[1]; // Calculate Kalman gains - float SK_TAS = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][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[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][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[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= R_TAS) { + SK_TAS = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the wind state variances and try again next time + P[14][14] += 0.05f*R_TAS; + P[15][15] += 0.05f*R_TAS; + return; + } Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); From c04064fd6a40bc8ac9f396d4d0ba565b26eda6db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 10:53:18 +0200 Subject: [PATCH 059/139] sdlog2: Log minimalistic GPS SNR for first 16 satellites --- src/modules/sdlog2/sdlog2.c | 14 ++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b74d4183bd..d680833f82 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -834,6 +834,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_GSN0_s log_GSN0; + struct log_GSN1_s log_GSN1; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -982,6 +984,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; LOGBUFFER_WRITE_AND_COUNT(GPS); + + /* log the SNR of each satellite for a detailed view of signal quality */ + log_msg.msg_type = LOG_GSN0_MSG; + /* pick the smaller number so we do not overflow any of the arrays */ + unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]); + unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < sat_max_snr; i++) { + log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + } + LOGBUFFER_WRITE_AND_COUNT(GSN0); } /* --- SENSOR COMBINED --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 595a787d69..0c61886579 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -317,6 +317,18 @@ struct log_VICN_s { float yaw; }; +/* --- GSN0 - GPS SNR #0 --- */ +#define LOG_GSN0_MSG 26 +struct log_GSN0_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + +/* --- GSN1 - GPS SNR #1 --- */ +#define LOG_GSN1_MSG 27 +struct log_GSN1_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -368,6 +380,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From c9162f428ae9e355cf3ad800070e0977a78c35c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 10:53:54 +0200 Subject: [PATCH 060/139] sdlog2: Remove an unreachable comparison --- 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 d680833f82..39f433eb53 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -684,7 +684,7 @@ int sdlog2_thread_main(int argc, char *argv[]) case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r <= 0) { + if (r == 0) { r = 1; } From e5508a1aa07fb4a0e76dfd077c5beaa114f21695 Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 09:41:41 -0700 Subject: [PATCH 061/139] Add Gumstix AeroCore device Based on the work of Andrew Smith [1], add board configuration and device drivers to support the Gumstix AeroCore (previously Aerodroid) board [2]. The AeroCore is an autopilot board based on a STM32F427 similar to the FMUv2. [1] https://github.com/smithandrewc/Firmware [2] https://store.gumstix.com/index.php/products/585/ Signed-off-by: Ash Charles --- Images/aerocore.prototype | 12 + makefiles/board_aerocore.mk | 11 + makefiles/config_aerocore_default.mk | 125 +++ nuttx-configs/aerocore/include/board.h | 263 +++++ nuttx-configs/aerocore/include/nsh_romfsimg.h | 42 + nuttx-configs/aerocore/nsh/Make.defs | 179 ++++ nuttx-configs/aerocore/nsh/appconfig | 42 + nuttx-configs/aerocore/nsh/defconfig | 950 ++++++++++++++++++ nuttx-configs/aerocore/nsh/setenv.sh | 67 ++ nuttx-configs/aerocore/scripts/ld.script | 150 +++ nuttx-configs/aerocore/src/Makefile | 84 ++ nuttx-configs/aerocore/src/empty.c | 4 + src/drivers/boards/aerocore/aerocore_init.c | 282 ++++++ src/drivers/boards/aerocore/aerocore_led.c | 121 +++ .../boards/aerocore/aerocore_pwm_servo.c | 117 +++ src/drivers/boards/aerocore/aerocore_spi.c | 183 ++++ src/drivers/boards/aerocore/board_config.h | 176 ++++ src/drivers/boards/aerocore/module.mk | 8 + src/drivers/boards/px4fmu-v1/board_config.h | 2 + src/drivers/boards/px4fmu-v2/board_config.h | 2 + src/drivers/drv_gpio.h | 10 +- src/drivers/drv_gps.h | 4 + src/drivers/gps/gps.cpp | 11 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 84 +- src/drivers/stm32/adc/adc.cpp | 4 + src/drivers/stm32/drv_hrt.c | 4 +- src/lib/version/version.h | 4 + src/modules/commander/commander_helper.cpp | 2 +- src/modules/sensors/sensor_params.c | 9 + src/modules/sensors/sensors.cpp | 10 +- src/systemcmds/mtd/mtd.c | 4 + 33 files changed, 2960 insertions(+), 10 deletions(-) create mode 100644 Images/aerocore.prototype create mode 100644 makefiles/board_aerocore.mk create mode 100644 makefiles/config_aerocore_default.mk create mode 100644 nuttx-configs/aerocore/include/board.h create mode 100644 nuttx-configs/aerocore/include/nsh_romfsimg.h create mode 100644 nuttx-configs/aerocore/nsh/Make.defs create mode 100644 nuttx-configs/aerocore/nsh/appconfig create mode 100644 nuttx-configs/aerocore/nsh/defconfig create mode 100755 nuttx-configs/aerocore/nsh/setenv.sh create mode 100644 nuttx-configs/aerocore/scripts/ld.script create mode 100644 nuttx-configs/aerocore/src/Makefile create mode 100644 nuttx-configs/aerocore/src/empty.c create mode 100644 src/drivers/boards/aerocore/aerocore_init.c create mode 100644 src/drivers/boards/aerocore/aerocore_led.c create mode 100644 src/drivers/boards/aerocore/aerocore_pwm_servo.c create mode 100644 src/drivers/boards/aerocore/aerocore_spi.c create mode 100644 src/drivers/boards/aerocore/board_config.h create mode 100644 src/drivers/boards/aerocore/module.mk diff --git a/Images/aerocore.prototype b/Images/aerocore.prototype new file mode 100644 index 0000000000..8502a08646 --- /dev/null +++ b/Images/aerocore.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 19, + "magic": "AeroCore", + "description": "Firmware for the Gumstix AeroCore board", + "image": "", + "build_time": 0, + "summary": "AEROCORE", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/makefiles/board_aerocore.mk b/makefiles/board_aerocore.mk new file mode 100644 index 0000000000..6f4b932666 --- /dev/null +++ b/makefiles/board_aerocore.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the Gumstix AeroCore +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = AEROCORE + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk new file mode 100644 index 0000000000..e038500a6a --- /dev/null +++ b/makefiles/config_aerocore_default.mk @@ -0,0 +1,125 @@ +# +# Makefile for the AeroCore *default* configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/boards/aerocore +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/ms5611 +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/boardinfo +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/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/fw_att_pos_estimator +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# 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 +MODULES += lib/launchdetection + +# +# 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 + +BUILTIN_COMMANDS := \ + $(call _B, hello, , 2048, hello_main) \ + $(call _B, i2c, , 2048, i2c_main) diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h new file mode 100644 index 0000000000..8705c1bc25 --- /dev/null +++ b/nuttx-configs/aerocore/include/board.h @@ -0,0 +1,263 @@ +/************************************************************************************ + * configs/aerocore/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The AeroCore uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (24,000,000 / 24) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +/* USART1 on PB[6,7]: GPS */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +/* USART2 on PD[5,6]: J5 Breakout */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_CTS 0 // unused +#define GPIO_USART2_RTS 0 // unused + +/* USART3 on PD[8,9]: to DuoVero UART2 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_CTS 0 // unused +#define GPIO_USART3_RTS 0 // unused + +/* UART7 on PE[78]: J7 Breakout */ +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* + * UART8 on PE[0-1]: System Console on Port C of USB (J7) + * No alternate pin config +*/ + +/* USART[1,6] require a RX DMA configuration */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ + +/* PB[10-11]: I2C2 is broken out on J9 header */ +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * SPI + */ +/* PA[4-7] SPI1 broken out on J12 */ +#define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) + +/* PB[12-15]: SPI2 connected to DuoVero SPI1 */ +#define GPIO_SPI2_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */ +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) + +/* PC[10-12]: SPI3 connected to onboard sensors */ +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz) + +/* PE[11-14]: SPI4 connected to FRAM */ +#define GPIO_SPI4_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */ +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/aerocore/include/nsh_romfsimg.h b/nuttx-configs/aerocore/include/nsh_romfsimg.h new file mode 100644 index 0000000000..15e4e7a8d5 --- /dev/null +++ b/nuttx-configs/aerocore/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs new file mode 100644 index 0000000000..c7a1b71bbd --- /dev/null +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/aerocore/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/aerocore/nsh/appconfig b/nuttx-configs/aerocore/nsh/appconfig new file mode 100644 index 0000000000..2850dac06e --- /dev/null +++ b/nuttx-configs/aerocore/nsh/appconfig @@ -0,0 +1,42 @@ +############################################################################ +# configs/aerocore/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig new file mode 100644 index 0000000000..8d0bae7b9f --- /dev/null +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -0,0 +1,950 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +# CONFIG_STM32_I2C1 is not set +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +# CONFIG_STM32_OTGFS is not set +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI4=y +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +# CONFIG_STM32_TIM10 is not set +# CONFIG_STM32_TIM11 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=36 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +CONFIG_RAMTRON_FUJITSU=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_BAUD=115200 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_BAUD=115200 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +CONFIG_EXAMPLES_HELLO=y +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MTDPART is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +CONFIG_SYSTEM_I2CTOOL=y +CONFIG_I2CTOOL_MINBUS=0 +CONFIG_I2CTOOL_MAXBUS=3 +CONFIG_I2CTOOL_MINADDR=0x03 +CONFIG_I2CTOOL_MAXADDR=0x77 +CONFIG_I2CTOOL_MAXREGADDR=0xff +CONFIG_I2CTOOL_DEFFREQ=4000000 + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/aerocore/nsh/setenv.sh b/nuttx-configs/aerocore/nsh/setenv.sh new file mode 100755 index 0000000000..2327f38a1b --- /dev/null +++ b/nuttx-configs/aerocore/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/aerocore/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/aerocore/scripts/ld.script b/nuttx-configs/aerocore/scripts/ld.script new file mode 100644 index 0000000000..968d3127ff --- /dev/null +++ b/nuttx-configs/aerocore/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/aerocore/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/aerocore/src/Makefile b/nuttx-configs/aerocore/src/Makefile new file mode 100644 index 0000000000..859ba4ab23 --- /dev/null +++ b/nuttx-configs/aerocore/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/aerocore/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c new file mode 100644 index 0000000000..ace900866c --- /dev/null +++ b/nuttx-configs/aerocore/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c new file mode 100644 index 0000000000..4e3ba2d7e4 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_init.c + * + * AeroCore-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi3; +static struct spi_dev_s *spi4; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + + /* Configure Sensors on SPI bus #3 */ + spi3 = up_spiinitialize(3); + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: 1MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi3, 10000000); + SPI_SETBITS(spi3, 8); + SPI_SETMODE(spi3, SPIDEV_MODE3); + SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi3, PX4_SPIDEV_BARO, false); + up_udelay(20); + message("[boot] Initialized SPI port 3 (SENSORS)\n"); + + /* Configure FRAM on SPI bus #4 */ + spi4 = up_spiinitialize(4); + if (!spi4) { + message("[boot] FAILED to initialize SPI port 4\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: ~10MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE0); + SPI_SELECT(spi4, SPIDEV_FLASH, false); + message("[boot] Initialized SPI port 4 (FRAM)\n"); + + return OK; +} diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c new file mode 100644 index 0000000000..e40d1730c6 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file aerocore_led.c + * + * AeroCore LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + stm32_configgpio(GPIO_LED0); + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_off(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, false); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, false); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_toggle(int led) +{ + switch (led) { + case 0: + if (stm32_gpioread(GPIO_LED0)) + stm32_gpiowrite(GPIO_LED0, false); + else + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c new file mode 100644 index 0000000000..251eaff7b4 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1500, + } +}; diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c new file mode 100644 index 0000000000..e329bd9d1a --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI1_NSS); + stm32_gpiowrite(GPIO_SPI1_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI2_NSS); + stm32_gpiowrite(GPIO_SPI2_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI3 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); +#endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI4_NSS); + stm32_gpiowrite(GPIO_SPI4_NSS, 1); +#endif +} + +#ifdef CONFIG_STM32_SPI1 +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI1_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI2_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI3 +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI4_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h new file mode 100644 index 0000000000..a3257f3397 --- /dev/null +++ b/src/drivers/boards/aerocore/board_config.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * AeroCore internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* LEDs */ +#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) + +/* Gyro */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) + +/* Accel & Mag */ +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2) + +/* GPS */ +#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4) + +/* SPI3--Sensors */ +#define PX4_SPI_BUS_SENSORS 3 +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +/* Nominal chip selects for devices on SPI bus #3 */ +#define PX4_SPIDEV_ACCEL_MAG 0 +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_BARO 2 + +/* User GPIOs broken out on J11 */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* PWM + * + * Eight PWM outputs are configured. + * + * Pins: + * + * CH1 : PA8 : TIM1_CH1 + * CH2 : PA9 : TIM1_CH2 + * CH3 : PA10 : TIM1_CH3 + * CH4 : PA11 : TIM1_CH4 + * CH5 : PC6 : TIM3_CH1 + * CH6 : PC7 : TIM3_CH2 + * CH7 : PC8 : TIM3_CH3 + * CH8 : PC9 : TIM3_CH4 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1 +#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3 +#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 +#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 +#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer 8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* Tone Alarm (no onboard speaker )*/ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk new file mode 100644 index 0000000000..b53fe0a293 --- /dev/null +++ b/src/drivers/boards/aerocore/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for the AeroCore +# + +SRCS = aerocore_init.c \ + aerocore_pwm_servo.c \ + aerocore_spi.c \ + aerocore_led.c diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 02c26b5c0b..58273f2d29 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -85,6 +85,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define PX4_SPI_BUS_SENSORS 1 + /* * Use these in place of the spi_dev_e enumeration to * select a specific SPI device on SPI1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7cfca7656e..c2de1bfba0 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -107,6 +107,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define PX4_SPI_BUS_SENSORS 1 + /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f60964c2b1..5acd0d3434 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,14 @@ #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +/* + * AeroCore GPIO numbers and configuration. + * + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* no GPIO driver on the PX4IOv1 board */ #endif @@ -146,4 +154,4 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) -#endif /* _DRV_GPIO_H */ \ No newline at end of file +#endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 06e3535b30..96669a0214 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -46,7 +46,11 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" +#else #define GPS_DEFAULT_UART_PORT "/dev/ttyS3" +#endif #define GPS_DEVICE_PATH "/dev/gps" diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..3377055705 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,8 @@ #include #include +#include + #include "ubx.h" #include "mtk.h" @@ -421,7 +423,14 @@ GPS::task_main() void GPS::cmd_reset() { - //XXX add reset? +#ifdef GPIO_GPS_NRESET + warnx("Toggling GPS reset pin"); + stm32_configgpio(GPIO_GPS_NRESET); + stm32_gpiowrite(GPIO_GPS_NRESET, 0); + usleep(100); + stm32_gpiowrite(GPIO_GPS_NRESET, 1); + warnx("Toggled GPS reset pin"); +#endif } void diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4dee7649bc..4ca8b5e42d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1793,7 +1793,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 26216e840f..8759d16a16 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -117,7 +117,7 @@ private: device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf) { - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d72ead9bb..1b6b136eb8 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -92,6 +92,7 @@ public: MODE_2PWM, MODE_4PWM, MODE_6PWM, + MODE_8PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -113,6 +114,9 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + static const unsigned _max_actuators = 8; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -203,6 +207,21 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + /* AeroCore breaks out User GPIOs on J11 */ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, +// {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -382,6 +401,20 @@ PX4FMU::set_mode(Mode mode) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs + debug("MODE_8PWM"); + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xff); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; +#endif + case MODE_NONE: debug("MODE_NONE"); @@ -602,6 +635,9 @@ PX4FMU::task_main() num_outputs = 6; break; + case MODE_8PWM: + num_outputs = 8; + break; default: num_outputs = 0; break; @@ -757,6 +793,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_2PWM: case MODE_4PWM: case MODE_6PWM: +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: +#endif ret = pwm_ioctl(filp, cmd, arg); break; @@ -986,6 +1025,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): + case PWM_SERVO_SET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1013,6 +1061,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { @@ -1040,12 +1097,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(3): case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET_RATEGROUP(6): + case PWM_SERVO_GET_RATEGROUP(7): +#endif *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: + *(unsigned *)arg = 8; + break; +#endif + case MODE_6PWM: *(unsigned *)arg = 6; break; @@ -1091,6 +1158,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_6PWM); break; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + case 8: + set_mode(MODE_8PWM); + break; +#endif default: ret = -EINVAL; @@ -1181,10 +1253,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) unsigned count = len / 2; uint16_t values[6]; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { + // we have at most 8 outputs + count = 8; + } +#else if (count > 6) { // we have at most 6 outputs count = 6; } +#endif // allow for misaligned values memcpy(values, buffer, count * 2); @@ -1458,6 +1537,9 @@ fmu_new_mode(PortMode new_mode) #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + servo_mode = PX4FMU::MODE_8PWM; #endif break; @@ -1776,7 +1858,7 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 3a60d2cae1..de13b89697 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -419,6 +419,10 @@ adc_main(int argc, char *argv[]) g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE + /* XXX this hardcodes the default channel set for AeroCore - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 5bb5502796..281f918d70 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -141,7 +141,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP -# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN # if CONFIG_STM32_TIM10 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 # endif @@ -150,7 +150,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN # if CONFIG_STM32_TIM11 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif diff --git a/src/lib/version/version.h b/src/lib/version/version.h index af733aaf06..d8ccb67748 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,4 +59,8 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define HW_ARCH "AEROCORE" +#endif + #endif /* VERSION_H_ */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 86f77cdb81..940a04aa1f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -206,7 +206,7 @@ int led_init() return ERROR; } - /* the blue LED is only available on FMUv1 but not FMUv2 */ + /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); /* we consider the amber led mandatory */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 07be3560a9..c021e1fde9 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#elif CONFIG_ARCH_BOARD_AEROCORE +/** + * Scaling factor for battery voltage sensor on AeroCore. + * + * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); #else /** * Scaling factor for battery voltage sensor on FMU v1. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 18bf97f8da..d310161114 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -126,6 +126,12 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + #define BATT_V_LOWPASS 0.001f #define BATT_V_IGNORE_THRESHOLD 3.5f @@ -797,7 +803,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -806,7 +812,7 @@ Sensors::accel_init() ioctl(fd, SENSORIOCSPOLLRATE, 800); #else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE #endif diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0a88d40f39..a57eaafe71 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -160,7 +160,11 @@ static void ramtron_attach(void) { /* find the right spi */ +#ifdef CONFIG_ARCH_BOARD_AEROCORE + struct spi_dev_s *spi = up_spiinitialize(4); +#else struct spi_dev_s *spi = up_spiinitialize(2); +#endif /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); From 3b72e31e83d1075096a7ee99896885998bd3d6fd Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 09:51:37 -0700 Subject: [PATCH 062/139] [l3gd20] Add support for L3G4200D chip The L3G4200D chip is very similar to the L3GD20[H] parts and can use the same driver with minor adjustments. There are four differences: * WHO_AM_I register is 0xD3 (not 0xD4 or 0xD7): - added an extra case to the driver probe * Sampling rates are marginally different: - setting sampling rate now depends on the detected chip * I2C address range is different: - no changes as the driver doesn't support i2c access * the L3G4200D has a self-test function: - no changes---chose not to implement feature in driver Signed-off-by: Ash Charles --- src/drivers/l3gd20/l3gd20.cpp | 60 +++++++++++++++++++++++++++++------ 1 file changed, 50 insertions(+), 10 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 90c3db9ae9..a4d4c6c831 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -34,6 +34,9 @@ /** * @file l3gd20.cpp * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * + * Note: With the exception of the self-test feature, the ST L3G4200D is + * also supported by this driver. */ #include @@ -89,9 +92,11 @@ static const int ERROR = -1; #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 +#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */ #define ADDR_CTRL_REG1 0x20 #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ + /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) @@ -166,6 +171,7 @@ static const int ERROR = -1; #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) #define L3GD20_DEFAULT_RATE 760 +#define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 @@ -216,6 +222,9 @@ private: math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_z; + /* true if an L3G4200D is detected */ + bool _is_l3g4200d; + /** * Start automatic measurement. */ @@ -331,7 +340,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), - _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _is_l3g4200d(false) { // enable debug() calls _debug_enabled = true; @@ -417,8 +427,11 @@ L3GD20::probe() _orientation = SENSOR_BOARD_ROTATION_270_DEG; #elif CONFIG_ARCH_BOARD_PX4FMU_V2 _orientation = SENSOR_BOARD_ROTATION_270_DEG; + /* AeroCore won't reach here but the pre-processor doesn't know this so it hits the error condition */ + #elif CONFIG_ARCH_BOARD_AEROCORE + _orientation = SENSOR_BOARD_ROTATION_270_DEG; #else - #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE #endif success = true; @@ -430,6 +443,13 @@ L3GD20::probe() success = true; } + /* Detect the L3G4200D used on AeroCore */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + _is_l3g4200d = true; + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + success = true; + } + if (success) return OK; @@ -502,6 +522,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: + if (_is_l3g4200d) + return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ @@ -683,23 +705,41 @@ L3GD20::set_samplerate(unsigned frequency) uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0) - frequency = 760; + if (_is_l3g4200d) + frequency = 800; + else + frequency = 760; - /* use limits good for H or non-H models */ + /* + * Use limits good for H or non-H models. Rates are slightly different + * for L3G4200D part but register settings are the same. + */ if (frequency <= 100) { - _current_rate = 95; + if (_is_l3g4200d) + _current_rate = 100; + else + _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 200) { - _current_rate = 190; + if (_is_l3g4200d) + _current_rate = 200; + else + _current_rate = 190; bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { - _current_rate = 380; + if (_is_l3g4200d) + _current_rate = 400; + else + _current_rate = 380; bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { - _current_rate = 760; + if (_is_l3g4200d) + _current_rate = 800; + else + _current_rate = 760; bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; @@ -772,7 +812,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(0); // 760Hz + set_samplerate(0); // 760Hz or 800Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); @@ -971,7 +1011,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; From 9db966e05876f35d56cecdfc2c74f2df914bd8bb Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 13:47:40 -0700 Subject: [PATCH 063/139] [gps] Conditionally set default GPS port AeroCore uses ttyS0 not ttyS3 as the serial port connected to the GPS. Now, a board can set GPS_DEFAULT_UART_PORT to override the default setting in a board-specific fashion. Signed-off-by: Ash Charles --- src/drivers/boards/aerocore/board_config.h | 1 + src/drivers/drv_gps.h | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index a3257f3397..dd98cf4801 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -73,6 +73,7 @@ __BEGIN_DECLS /* GPS */ #define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4) +#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" /* SPI3--Sensors */ #define PX4_SPI_BUS_SENSORS 3 diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 96669a0214..e14f4e00da 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -43,12 +43,12 @@ #include #include +#include "board_config.h" + #include "drv_sensor.h" #include "drv_orb_dev.h" -#ifdef CONFIG_ARCH_BOARD_AEROCORE -#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" -#else +#ifndef GPS_DEFAULT_UART_PORT #define GPS_DEFAULT_UART_PORT "/dev/ttyS3" #endif From 7d0850a710b3ac9e9e165beb36389577d0e5adcb Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 13:52:44 -0700 Subject: [PATCH 064/139] [l3gd20] Style fixes for l3g4200d integration As requested here [1], this fixes some of the style errors introduced with the addition of l3g4200d support to the l3gd20 driver. Gyro orientation is set conditionally for the aerocore board. [1] https://github.com/PX4/Firmware/pull/937 Signed-off-by: Ash Charles --- src/drivers/l3gd20/l3gd20.cpp | 32 ++++++++++---------------------- 1 file changed, 10 insertions(+), 22 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a4d4c6c831..b9571dc4b9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -446,7 +446,9 @@ L3GD20::probe() /* Detect the L3G4200D used on AeroCore */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { _is_l3g4200d = true; - _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #ifdef CONFIG_ARCH_BOARD_AEROCORE + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #endif success = true; } @@ -522,8 +524,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - if (_is_l3g4200d) + if (_is_l3g4200d) { return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); + } return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ @@ -705,41 +708,26 @@ L3GD20::set_samplerate(unsigned frequency) uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; if (frequency == 0) - if (_is_l3g4200d) - frequency = 800; - else - frequency = 760; + frequency = _is_l3g4200d ? 800 : 760; /* * Use limits good for H or non-H models. Rates are slightly different * for L3G4200D part but register settings are the same. */ if (frequency <= 100) { - if (_is_l3g4200d) - _current_rate = 100; - else - _current_rate = 95; + _current_rate = _is_l3g4200d ? 100 : 95; bits |= RATE_95HZ_LP_25HZ; } else if (frequency <= 200) { - if (_is_l3g4200d) - _current_rate = 200; - else - _current_rate = 190; + _current_rate = _is_l3g4200d ? 200 : 190; bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { - if (_is_l3g4200d) - _current_rate = 400; - else - _current_rate = 380; + _current_rate = _is_l3g4200d ? 400 : 380; bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { - if (_is_l3g4200d) - _current_rate = 800; - else - _current_rate = 760; + _current_rate = _is_l3g4200d ? 800 : 760; bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; From 2d29c5bd72f77d572a8f83480e841051236b3645 Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 14:06:59 -0700 Subject: [PATCH 065/139] [aerocore] Remove commented code for GPIO2 GPIO2 is currently used for the tone alarm, not an arbitrary GPIO. Signed-off-by: Ash Charles --- src/drivers/boards/aerocore/board_config.h | 2 -- src/drivers/px4fmu/fmu.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index dd98cf4801..814065fcbc 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -89,7 +89,6 @@ __BEGIN_DECLS /* User GPIOs broken out on J11 */ #define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) #define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) -//#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) #define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) #define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) #define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) @@ -102,7 +101,6 @@ __BEGIN_DECLS #define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) #define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) -//#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) #define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 1b6b136eb8..fd69cf795b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -211,7 +211,6 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { /* AeroCore breaks out User GPIOs on J11 */ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, -// {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, From 8d3fed09443faa6a3c79b68b7800ed3472877a1c Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Tue, 13 May 2014 19:59:44 -0400 Subject: [PATCH 066/139] Reduce potential dataman memory fragmentation The data manager dynamically allocates relatively small work item blocks on an as needed basis. It never frees these, instead maintaining then in a list of available block for reuse when needed. Even if these blocks are small, the are required at non-deterministic times and can end up scattered in memory thus causing memory fragmentation. In order to mitigate this problems work item blocks are allocated in groups of 8 in contiguous memory to reduce the number of scattered memory allocations. In reality, based on current usage, rarely will more than one group of 8 be allocated. --- src/modules/dataman/dataman.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 7505ba3584..cbd9b2d7cd 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -100,6 +100,8 @@ typedef struct { }; } work_q_item_t; +const size_t k_work_item_allocation_chunk_size = 8; + /* Usage statistics */ static unsigned g_func_counts[dm_number_of_funcs]; @@ -177,9 +179,20 @@ create_work_item(void) unlock_queue(&g_free_q); - /* If we there weren't any free items then obtain memory for a new one */ - if (item == NULL) - item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we there weren't any free items then obtain memory for a new ones */ + if (item == NULL) { + item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); + if (item) { + lock_queue(&g_free_q); + for (int i = 1; i < k_work_item_allocation_chunk_size; i++) + sq_addfirst(&(item + i)->link, &(g_free_q.q)); + /* Update the queue size and potentially the maximum queue size */ + g_free_q.size += k_work_item_allocation_chunk_size - 1; + if (g_free_q.size > g_free_q.max_size) + g_free_q.max_size = g_free_q.size; + unlock_queue(&g_free_q); + } + } /* If we got one then lock the item*/ if (item) From cd9a72e391948fbf620c8cb129020ae7ecc9cab3 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Tue, 13 May 2014 20:24:19 -0400 Subject: [PATCH 067/139] Free data manager work items the same way they were allocated Since data manager work items are allocated in groups of 8, they need to be freed the same way should the manager need to stop. --- src/modules/dataman/dataman.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index cbd9b2d7cd..16703899db 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -75,7 +75,8 @@ typedef enum { typedef struct { sq_entry_t link; /**< list linkage */ sem_t wait_sem; - dm_function_t func; + unsigned char first; + unsigned char func; ssize_t result; union { struct { @@ -183,9 +184,12 @@ create_work_item(void) if (item == NULL) { item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); if (item) { + item->first = 1; lock_queue(&g_free_q); - for (int i = 1; i < k_work_item_allocation_chunk_size; i++) + for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); + } /* Update the queue size and potentially the maximum queue size */ g_free_q.size += k_work_item_allocation_chunk_size - 1; if (g_free_q.size > g_free_q.max_size) @@ -730,8 +734,8 @@ task_main(int argc, char *argv[]) for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; - - free(work); + if (work->first) + free(work); } destroy_q(&g_work_q); From 97fb361ea9e8d0952327a51b3400401d96f08506 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 May 2014 11:25:37 +0200 Subject: [PATCH 068/139] Set an RC status flag so that we can read out the RC status in parallel to the RC data --- src/modules/px4iofirmware/controls.c | 7 ++++++- src/modules/px4iofirmware/protocol.h | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 56c5aa0051..62e6b12cbc 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -291,6 +291,7 @@ controls_tick() { /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; /* if we have enough channels (5) to control the vehicle, the mapping is ok */ if (assigned_channels > 4) { @@ -336,6 +337,9 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* flag raw RC as lost */ + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; @@ -405,8 +409,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) if (*num_values > PX4IO_RC_INPUT_CHANNELS) *num_values = PX4IO_RC_INPUT_CHANNELS; - for (unsigned i = 0; i < *num_values; i++) + for (unsigned i = 0; i < *num_values; i++) { values[i] = ppm_buffer[i]; + } /* clear validity */ ppm_last_valid_decode = 0; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6c20d6006d..91975f2a07 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -142,6 +142,7 @@ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ From 8deb1c9160ae02828ce5bc810a4262daca94b530 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 May 2014 11:26:09 +0200 Subject: [PATCH 069/139] Read out the RC status at the same transfer as the channels to ensure we got synchronized data --- src/drivers/px4io/px4io.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index aec6dd3b75..e937f940ad 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1477,12 +1477,14 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; /* rc_lost has to be set before the call to this function */ - if (!input_rc.rc_lost && !input_rc.rc_failsafe) + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { _rc_last_valid = input_rc.timestamp_publication; + } input_rc.timestamp_last_signal = _rc_last_valid; From a62ac72b2e3e99b9104012cc936beb9426de2478 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 12:48:54 +0200 Subject: [PATCH 070/139] px4io: Hotfix for IO driver, do not rely on the reported channel count to limit array lengths --- src/drivers/px4io/px4io.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e937f940ad..c54f6fb3f5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1448,7 +1448,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; /* @@ -1456,8 +1456,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp_publication = hrt_absolute_time(); - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1469,17 +1467,27 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = regs[PX4IO_P_RAW_RC_COUNT]; - if (channel_count != _rc_chan_count) + /* limit the channel count */ + if (channel_count > RC_INPUT_MAX_CHANNELS) { + channel_count = RC_INPUT_MAX_CHANNELS; + } + + /* count channel count changes to identify signal integrity issues */ + if (channel_count != _rc_chan_count) { perf_count(_perf_chan_count); + } _rc_chan_count = channel_count; + input_rc.timestamp_publication = hrt_absolute_time(); + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + input_rc.channel_count = channel_count; /* rc_lost has to be set before the call to this function */ if (!input_rc.rc_lost && !input_rc.rc_failsafe) { @@ -1488,6 +1496,9 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) input_rc.timestamp_last_signal = _rc_last_valid; + /* FIELDS NOT SET HERE */ + /* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */ + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1495,8 +1506,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) return ret; } - input_rc.channel_count = channel_count; - memcpy(input_rc.values, ®s[prolog], channel_count * 2); + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + input_rc.values[i] = ®s[prolog + i]; + } return ret; } From c2f825647e8fd2aa4bf7a37399203c6071239765 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 13:06:20 +0200 Subject: [PATCH 071/139] px4io driver: Small fix --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c54f6fb3f5..f6b2af324a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1508,7 +1508,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* last thing set are the actual channel values as 16 bit values */ for (unsigned i = 0; i < channel_count; i++) { - input_rc.values[i] = ®s[prolog + i]; + input_rc.values[i] = regs[prolog + i]; } return ret; From 332e08b44a1081bf4e6fa969fa6c90c83e3e75bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:00:31 +0200 Subject: [PATCH 072/139] px4io driver: Deallocate perf counters in destructor properly --- src/drivers/px4io/px4io.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6b2af324a..3c3b598627 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -529,6 +529,11 @@ PX4IO::~PX4IO() if (_interface != nullptr) delete _interface; + /* deallocate perfs */ + perf_free(_perf_update); + perf_free(_perf_write); + perf_free(_perf_chan_count); + g_dev = nullptr; } From 15eaa3aed499941599c28e79dd40281a2de739c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:00:51 +0200 Subject: [PATCH 073/139] eeprom driver: Use less excessive perf counters on EEPROM --- src/systemcmds/mtd/24xxxx_mtd.c | 36 ++++++++++++++------------------- 1 file changed, 15 insertions(+), 21 deletions(-) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index e34be44e31..a3f9dfffff 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -142,12 +142,9 @@ struct at24c_dev_s { uint16_t pagesize; /* 32, 63 */ uint16_t npages; /* 128, 256, 512, 1024 */ - perf_counter_t perf_reads; - perf_counter_t perf_writes; - perf_counter_t perf_resets; - perf_counter_t perf_read_retries; - perf_counter_t perf_read_errors; - perf_counter_t perf_write_errors; + perf_counter_t perf_transfers; + perf_counter_t perf_resets_retries; + perf_counter_t perf_errors; }; /************************************************************************************ @@ -298,9 +295,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, for (;;) { - perf_begin(priv->perf_reads); + perf_begin(priv->perf_transfers); ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); + perf_end(priv->perf_transfers); if (ret >= 0) break; @@ -314,10 +311,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, * XXX maybe do special first-read handling with optional * bus reset as well? */ - perf_count(priv->perf_read_retries); + perf_count(priv->perf_resets_retries); if (--tries == 0) { - perf_count(priv->perf_read_errors); + perf_count(priv->perf_errors); return ERROR; } } @@ -383,9 +380,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t for (;;) { - perf_begin(priv->perf_writes); + perf_begin(priv->perf_transfers); ret = I2C_TRANSFER(priv->dev, &msgv[0], 1); - perf_end(priv->perf_writes); + perf_end(priv->perf_transfers); if (ret >= 0) break; @@ -397,7 +394,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t * poll for write completion. */ if (--tries == 0) { - perf_count(priv->perf_write_errors); + perf_count(priv->perf_errors); return ERROR; } } @@ -521,12 +518,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { priv->mtd.ioctl = at24c_ioctl; priv->dev = dev; - priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read"); - priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write"); - priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset"); - priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries"); - priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors"); - priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); + priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans"); + priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst"); + priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs"); } /* attempt to read to validate device is present */ @@ -548,9 +542,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { } }; - perf_begin(priv->perf_reads); + perf_begin(priv->perf_transfers); int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); - perf_end(priv->perf_reads); + perf_end(priv->perf_transfers); if (ret < 0) { return NULL; From 6018ffa462ad3e78d3cc4514b1a6cff5cf83961f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:02:01 +0200 Subject: [PATCH 074/139] nshterm: Use only the stack we really need --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a485885354..fe79ec3b76 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 3000 +MODULE_STACKSIZE = 1500 From a1aa8e84ffc4fb5d8f51df09a0cb70c569cd8760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:04:58 +0200 Subject: [PATCH 075/139] Reduce top stack usage --- src/systemcmds/top/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk index 9239aafc31..985ea846f8 100644 --- a/src/systemcmds/top/module.mk +++ b/src/systemcmds/top/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = top SRCS = top.c -MODULE_STACKSIZE = 3000 +MODULE_STACKSIZE = 2048 MAXOPTIMIZATION = -Os From 63905265ebccd3a2d7fd94d5cf4f2c02102481f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:05:47 +0200 Subject: [PATCH 076/139] Save code size on commander, reduce stack size of starting tool (NOT OF THE APP ITSELF!) --- src/modules/commander/module.mk | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 554dfcb080..27ca5c1823 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,3 +47,7 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp + +MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From e5d28b239393748a5ba2a9ca7ab1733f5333cbd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:15:40 +0200 Subject: [PATCH 077/139] Hotfix: Fixed wing default parameters contained an unknown name --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 4cd73e23fe..3a50fcf56f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,6 +11,4 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_PITCH 1 fi \ No newline at end of file From c646a8ff1d6fd0b0f08ed317619c20b83b49e418 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 14:23:16 +0200 Subject: [PATCH 078/139] Let the param set command default to non-failing, because unknown params are in the script default init not a reason to give up on the complete boot --- src/systemcmds/param/param.c | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 984d19bd9e..d92ee88efe 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -61,7 +61,7 @@ static void do_load(const char* param_file_name); static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); -static void do_set(const char* name, const char* val); +static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); static void do_reset(); @@ -117,10 +117,17 @@ param_main(int argc, char *argv[]) } if (!strcmp(argv[1], "set")) { - if (argc >= 4) { - do_set(argv[2], argv[3]); + if (argc >= 5) { + + /* if the fail switch is provided, fails the command if not found */ + bool fail = !strcmp(argv[4], "fail"); + + do_set(argv[2], argv[3], fail); + + } else if (argc >= 4) { + do_set(argv[2], argv[3], false); } else { - errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); + errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'"); } } @@ -282,7 +289,7 @@ do_show_print(void *arg, param_t param) } static void -do_set(const char* name, const char* val) +do_set(const char* name, const char* val, bool fail_on_not_found) { int32_t i; float f; @@ -290,8 +297,8 @@ do_set(const char* name, const char* val) /* set nothing if parameter cannot be found */ if (param == PARAM_INVALID) { - /* param not found */ - errx(1, "Error: Parameter %s not found.", name); + /* param not found - fail silenty in scripts as it prevents booting */ + errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name); } printf("%c %s: ", From 42a7d80a8119bd583e40ffda957d195fd7e81b9d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 May 2014 14:55:14 +0200 Subject: [PATCH 079/139] mc_att_control: limit max yaw setpoint offset --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a69153bf0b..e74546f113 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -506,6 +506,14 @@ MulticopterAttitudeControl::control_attitude(float dt) /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + if (yaw_offs < - yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs); + } _v_att_sp.R_valid = false; publish_att_sp = true; } From 1a1f7ff33bb3d50f8ecd67d56b58843ac4373fa8 Mon Sep 17 00:00:00 2001 From: Liio Chen Date: Wed, 14 May 2014 22:18:36 +0800 Subject: [PATCH 080/139] Fix error on some compiler --- src/modules/systemlib/pid/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 6a4e9392a2..45f218a5bc 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -53,7 +53,7 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min) { pid->mode = mode; pid->dt_min = dt_min; From cbc559b6d6285f169e352dde2a205be12aef9ea8 Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Wed, 14 May 2014 09:19:30 -0700 Subject: [PATCH 081/139] [l3gd20] Make gyro orientation board-overridable As discussed [1], provide a default SENSOR_BOARD_ROTATION (270 degrees as this seems most common) and let boards override it as necessary. [1] https://github.com/gumstix/m4-firmware/commit/7d0850a710b3ac9e9e165beb36389577d0e5adcb#commitcomment-6315550 Signed-off-by: Ash Charles --- src/drivers/boards/aerocore/board_config.h | 1 + src/drivers/l3gd20/l3gd20.cpp | 24 ++++++++-------------- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 814065fcbc..70142a3147 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -65,6 +65,7 @@ __BEGIN_DECLS /* Gyro */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) +#define SENSOR_BOARD_ROTATION_DEFAULT 3 /* SENSOR_BOARD_ROTATION_270_DEG */ /* Accel & Mag */ #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index b9571dc4b9..37e72388be 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -171,10 +171,14 @@ static const int ERROR = -1; #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) #define L3GD20_DEFAULT_RATE 760 -#define L3G4200D_DEFAULT_RATE 800 +#define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#ifndef SENSOR_BOARD_ROTATION_DEFAULT +#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG +#endif + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -333,7 +337,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _class_instance(-1), _current_rate(0), - _orientation(SENSOR_BOARD_ROTATION_270_DEG), + _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), @@ -423,17 +427,7 @@ L3GD20::probe() /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - /* AeroCore won't reach here but the pre-processor doesn't know this so it hits the error condition */ - #elif CONFIG_ARCH_BOARD_AEROCORE - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - #else - #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE - #endif - + _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } @@ -446,9 +440,7 @@ L3GD20::probe() /* Detect the L3G4200D used on AeroCore */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { _is_l3g4200d = true; - #ifdef CONFIG_ARCH_BOARD_AEROCORE - _orientation = SENSOR_BOARD_ROTATION_270_DEG; - #endif + _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } From 531ba79e55e9ccf9396ee30f067c933b4ec9c649 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:34:37 +0200 Subject: [PATCH 082/139] Reduce commander stack size mildly --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcdc..f92c7fc313 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2800, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From 7655f3e42556550e6769f6ac4b88304e1932c3ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:34:55 +0200 Subject: [PATCH 083/139] Reduce mavlink stack size slightly --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dd88b09490..d222f9edca 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2193,7 +2193,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1800, (main_t)&Mavlink::start_helper, (const char **)argv); From 25fd20487ec83e4edad7c88082d9283608f34e06 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:35:16 +0200 Subject: [PATCH 084/139] reduce stack size of dataman start handler --- src/modules/dataman/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index 27670dd3fd..234607b3de 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = dataman SRCS = dataman.c + +MODULE_STACKSIZE = 1200 From 07890300887319d1e3a85a30151a156f7b181f41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:35:54 +0200 Subject: [PATCH 085/139] Reduce stack size of navigator startup handler --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caac..6ea9dec2b2 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -45,3 +45,5 @@ SRCS = navigator_main.cpp \ geofence_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 1200 From 29ec1f388133a3bb11654b1f5accc9872d6ce6cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:36:11 +0200 Subject: [PATCH 086/139] Reduce stack size of sensors module start handler --- src/modules/sensors/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index aa538fd6b1..5b1bc5e86f 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -40,3 +40,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" SRCS = sensors.cpp \ sensor_params.c + +MODULE_STACKSIZE = 1200 From 972cf54c9637f9957f542ba7ca975fe449787946 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:36:37 +0200 Subject: [PATCH 087/139] Reduce stack size of PWM tool --- src/systemcmds/pwm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk index 4a23bba90e..13a24150f2 100644 --- a/src/systemcmds/pwm/module.mk +++ b/src/systemcmds/pwm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = pwm SRCS = pwm.c -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 1800 From 8962c272749108af4163e985109d9e760f0dd0f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:37:06 +0200 Subject: [PATCH 088/139] Reduce stack size of perf tool --- src/systemcmds/perf/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk index 77952842b3..ec39a7a85e 100644 --- a/src/systemcmds/perf/module.mk +++ b/src/systemcmds/perf/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = perf SRCS = perf.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1800 From d50ae8bb59635782f35f85f759a25d6f80c40663 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:37:27 +0200 Subject: [PATCH 089/139] Reduce stack size of preflight check tool --- src/systemcmds/preflight_check/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk index 7c3c887836..0cb2a4cd09 100644 --- a/src/systemcmds/preflight_check/module.mk +++ b/src/systemcmds/preflight_check/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = preflight_check SRCS = preflight_check.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1800 From 08a6e00cddd983f7fc91be3a0324f18f3e8f94f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 21:30:54 +0200 Subject: [PATCH 090/139] use a minimal sdlog2 buffer for FMUv1.x --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 3469d5f5f5..009e7431ac 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 8 -t + sdlog2 start -r 50 -a -b 5 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t From b216cc3cacc3ac3d89197a133f3235436304f4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 21:31:29 +0200 Subject: [PATCH 091/139] px4fmu: Give the FMU driver only the stack for init it needs --- src/drivers/px4fmu/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b33..eeb59e1a1f 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp + +MODULE_STACKSIZE = 1200 From 19d798addcd8565237b43cf6bac2a391063ef6bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 21:52:25 +0200 Subject: [PATCH 092/139] px4io driver: init stack only twice as big as really needed and not four times as big --- src/drivers/px4io/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 2054faa127..c14f1f7836 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -44,3 +44,5 @@ SRCS = px4io.cpp \ # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MODULE_STACKSIZE = 1200 From 18ed3cbbb8ba4eabd32db3d07c7480c1af22ebc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 22:13:33 +0200 Subject: [PATCH 093/139] Increase servo out rate via USB --- ROMFS/px4fmu_common/init.d/rc.usb | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index b7b5569453..afdba92afd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,8 +3,6 @@ # USB MAVLink start # -echo "Starting MAVLink on this USB console" - 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 @@ -12,7 +10,7 @@ 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 +mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 # Exit shell to make it available to MAVLink exit From 5466e68bb2f180ef7858d62b9840db1277497c4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 22:13:49 +0200 Subject: [PATCH 094/139] mavlink app: Use only the stack it needs to start --- src/modules/mavlink/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index dcca11977f..4ec6c02c83 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -48,3 +48,5 @@ SRCS += mavlink_main.cpp \ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 From 8dc0a21a7ee83dea5b3a3ac719c6a9e64aff78f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:14:58 +0200 Subject: [PATCH 095/139] mavlink, commander: Get back close to original stack sizes. Although tests came clean, we do not want to take any chances --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f92c7fc313..17d3d3dcdc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2800, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d222f9edca..611a8b804f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2193,7 +2193,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1800, + 2000, (main_t)&Mavlink::start_helper, (const char **)argv); From 19dc0b950916451c7dad287aed49113b72178c17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:15:41 +0200 Subject: [PATCH 096/139] dataman: Fix doxygen, no functional changes --- src/modules/dataman/dataman.c | 23 ++++++++++++----------- src/modules/dataman/dataman.h | 20 ++++++++++---------- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 7505ba3584..5f3ada0cdc 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Jean Cyr - * Lorenz Meier - * Julian Oes - * Thomas Gubler + * 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 @@ -37,6 +33,11 @@ /** * @file dataman.c * DATAMANAGER driver. + * + * @author Jean Cyr + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -62,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t __EXPORT int dm_clear(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); -/* Types of function calls supported by the worker task */ +/** Types of function calls supported by the worker task */ typedef enum { dm_write_func = 0, dm_read_func, @@ -71,7 +72,7 @@ typedef enum { dm_number_of_funcs } dm_function_t; -/* Work task work item */ +/** Work task work item */ typedef struct { sq_entry_t link; /**< list linkage */ sem_t wait_sem; @@ -411,7 +412,7 @@ _clear(dm_item_t item) return result; } -/* Tell the data manager about the type of the last reset */ +/** Tell the data manager about the type of the last reset */ static int _restart(dm_reset_reason reason) { @@ -480,7 +481,7 @@ _restart(dm_reset_reason reason) return result; } -/* write to the data manager file */ +/** Write to the data manager file */ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) { @@ -505,7 +506,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return (ssize_t)enqueue_work_item_and_wait_for_result(work); } -/* Retrieve from the data manager file */ +/** Retrieve from the data manager file */ __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { @@ -736,7 +737,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 4382baeb58..1dfb26f73d 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,7 +46,7 @@ extern "C" { #endif - /* Types of items that the data manager can store */ + /** Types of items that the data manager can store */ typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ @@ -56,7 +56,7 @@ extern "C" { DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; - /* The maximum number of instances for each item type */ + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, @@ -65,24 +65,24 @@ extern "C" { DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; - /* Data persistence levels */ + /** Data persistence levels */ typedef enum { DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ DM_PERSIST_VOLATILE /* Data does not survive resets */ } dm_persitence_t; - /* The reason for the last reset */ + /** The reason for the last reset */ typedef enum { DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ DM_INIT_REASON_VOLATILE /* Data does not survive reset */ } dm_reset_reason; - /* Maximum size in bytes of a single item instance */ + /** Maximum size in bytes of a single item instance */ #define DM_MAX_DATA_SIZE 124 - /* Retrieve from the data manager store */ + /** Retrieve from the data manager store */ __EXPORT ssize_t dm_read( dm_item_t item, /* The item type to retrieve */ @@ -91,7 +91,7 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* write to the data manager store */ + /** write to the data manager store */ __EXPORT ssize_t dm_write( dm_item_t item, /* The item type to store */ @@ -101,13 +101,13 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* Erase all items of this type */ + /** Erase all items of this type */ __EXPORT int dm_clear( dm_item_t item /* The item type to clear */ ); - /* Tell the data manager about the type of the last reset */ + /** Tell the data manager about the type of the last reset */ __EXPORT int dm_restart( dm_reset_reason restart_type /* The last reset type */ From 23fe9e6dc0cfa77eac0af1a789a22b2e56bdb84e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:22:36 +0200 Subject: [PATCH 097/139] navigator: comment only changes / code style fixes --- src/modules/navigator/geofence.cpp | 5 ++- src/modules/navigator/geofence.h | 5 ++- src/modules/navigator/geofence_params.c | 1 - .../navigator/mission_feasibility_checker.cpp | 5 ++- .../navigator/mission_feasibility_checker.h | 6 ++- src/modules/navigator/navigator_main.cpp | 6 +-- src/modules/navigator/navigator_mission.cpp | 3 +- src/modules/navigator/navigator_mission.h | 3 +- src/modules/navigator/navigator_params.c | 3 -- src/modules/navigator/navigator_state.h | 42 +++++++++++++++++-- 10 files changed, 56 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f452a85f7c..bc8dbca506 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr - * @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.cpp * Provides functions for handling the geofence + * + * @author Jean Cyr + * @author Thomas Gubler */ #include "geofence.h" diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9628b72711..2eb126ab56 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr - * @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.h * Provides functions for handling the geofence + * + * @author Jean Cyr + * @author Thomas Gubler */ #ifndef GEOFENCE_H_ diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca94..653b1ad849 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 646ab6ab9e..2f45f22679 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file mission_feasibility_checker.cpp * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier + * @author Thomas Gubler */ #include "mission_feasibility_checker.h" diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a2966..f98e284624 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +33,11 @@ /** * @file mission_feasibility_checker.h * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier + * @author Thomas Gubler */ + #ifndef MISSION_FEASIBILITY_CHECKER_H_ #define MISSION_FEASIBILITY_CHECKER_H_ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1a45e13450..c6b5112a5d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes - * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +31,7 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 72dddebfee..49fc627850 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.cpp * Helper class to access missions + * + * @author Julian Oes */ #include diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 2bd4da82eb..b0f88e0161 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.h * Helper class to access missions + * + * @author Julian Oes */ #ifndef NAVIGATOR_MISSION_H diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6dc..5139283b62 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h index 6a1475c9b0..476f934148 100644 --- a/src/modules/navigator/navigator_state.h +++ b/src/modules/navigator/navigator_state.h @@ -1,8 +1,42 @@ -/* - * navigator_state.h +/**************************************************************************** * - * Created on: 27.01.2014 - * Author: ton + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file navigator_state.h + * + * Navigator state + * + * @author Anton Babushkin */ #ifndef NAVIGATOR_STATE_H_ From 8d9c6fe4d7e54c4bc4cd0de4667519d805609c4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:25:23 +0200 Subject: [PATCH 098/139] mavlink app: Fix use of message buffer --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_main.h | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 611a8b804f..3e44102fb2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1523,6 +1523,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) void Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { + uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1bf51fd31e..c7a7d32f84 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -237,7 +237,6 @@ private: orb_advert_t _mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; From 61a3ddb4c202f0e8783c041d772a08a732194885 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:25:49 +0200 Subject: [PATCH 099/139] navigator: Reduce stack size by 50 bytes --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6b5112a5d..87c893079f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -848,7 +848,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&Navigator::task_main_trampoline, nullptr); From 5f96feb3e0928be58b96d5adbebc33e14ea2a690 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:27:59 +0200 Subject: [PATCH 100/139] mc_att_control: Code style fixes in comments --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 +++++---- src/modules/mc_att_control/mc_att_control_params.c | 7 ++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a69153bf0b..74e31dd5b9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +32,13 @@ ****************************************************************************/ /** - * @file mc_att_control_main.c + * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 19134c7b4c..ad38a0a030 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file mc_att_control_params.c * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin */ #include From 05f9336d20aa44ac3234ec13f984f81abb1264c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:28:18 +0200 Subject: [PATCH 101/139] mc_pos_control: Code style fixes in comments --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index dacdd46f0a..05ab5769bb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -35,6 +35,8 @@ /** * @file mc_pos_control_params.c * Multicopter position controller parameters. + * + * @author Anton Babushkin */ #include From 3f9028b728e64104aa21adfd1f0c3b16a80ba016 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:29:43 +0200 Subject: [PATCH 102/139] mc_att_control: Reduce stack mildly by 50 bytes --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 74e31dd5b9..9a0b726d41 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -818,7 +818,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); From 32f0b2c42263b5c02964ca6ee199e751e04a67a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:30:03 +0200 Subject: [PATCH 103/139] mc_pos_control: Reduce stack mildly by 50 bytes --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6e611d4ab2..09960d106e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1062,7 +1062,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); From f4279ccc0d563c353b8427f8d969bee6e86cdba7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:35:39 +0200 Subject: [PATCH 104/139] sensors: Reduce stack mildly by 50 bytes --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 18bf97f8da..8da6b06ef7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1669,7 +1669,7 @@ Sensors::start() _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&Sensors::task_main_trampoline, nullptr); From c59ca4d3b966e1e52297217d676582887ca04e41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:41:12 +0200 Subject: [PATCH 105/139] attitude_estimator_ekf: Code style fixes --- .../attitude_estimator_ekf_main.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 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 c61b6ff3fe..2ec889efe8 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * 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 @@ -34,9 +32,12 @@ ****************************************************************************/ /* - * @file attitude_estimator_ekf_main.c + * @file attitude_estimator_ekf_main.cpp * * Extended Kalman Filter for Attitude Estimation. + * + * @author Tobias Naegeli + * @author Lorenz Meier */ #include @@ -111,7 +112,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { From 9cd1fa5b51672c4c9eebaea2ccbc6e3ea69c02f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:41:33 +0200 Subject: [PATCH 106/139] attitude_estimator_so3: Code style fixes --- .../attitude_estimator_so3_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 3653defa66..e55b011608 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Hyon Lim - * Anton Babushkin + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /* * @file attitude_estimator_so3_main.cpp * + * @author Hyon Lim + * @author Anton Babushkin + * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. @@ -131,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { From 8e46308fdd65356e4e06ef9a91cc5771b7ac6f34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:52:22 +0200 Subject: [PATCH 107/139] examples: fixed wing: Reduce start tool stack size --- src/examples/fixedwing_control/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index d2c48934fc..a2a9eb1132 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = ex_fixedwing_control SRCS = main.c \ params.c + +MODULE_STACKSIZE = 1200 From 8f6a50708fe847fdfe7a1ea5c8d3f8771019e7c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:52:51 +0200 Subject: [PATCH 108/139] examples: Adjust start tool and main stack sizes to reasonable defaults --- src/examples/px4_daemon_app/module.mk | 2 ++ src/examples/px4_daemon_app/px4_daemon_app.c | 2 +- src/examples/px4_mavlink_debug/module.mk | 4 +++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk index 5f8aa73d57..fc42231425 100644 --- a/src/examples/px4_daemon_app/module.mk +++ b/src/examples/px4_daemon_app/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = px4_daemon_app SRCS = px4_daemon_app.c + +MODULE_STACKSIZE = 1200 diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 53f1b4a9a7..3eaf14148a 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("daemon", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 4096, + 2000, px4_daemon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk index fefd614961..c7ef97fc45 100644 --- a/src/examples/px4_mavlink_debug/module.mk +++ b/src/examples/px4_mavlink_debug/module.mk @@ -37,4 +37,6 @@ MODULE_COMMAND = px4_mavlink_debug -SRCS = px4_mavlink_debug.c \ No newline at end of file +SRCS = px4_mavlink_debug.c + +MODULE_STACKSIZE = 2000 From ec5dd5401e9654ec958a8e5eb2b77e9a375e44b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:53:13 +0200 Subject: [PATCH 109/139] attitude_estimator_ekf: Reduce start tool stack size --- src/modules/attitude_estimator_ekf/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index d98647f999..99d0c5bf28 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/cross.c + +MODULE_STACKSIZE = 1200 From ab154c9d3bd2f3034382c82b8613613af80287b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:53:30 +0200 Subject: [PATCH 110/139] attitude_estimator_so3: Reduce start tool stack size --- src/modules/attitude_estimator_so3/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk index e29bb16a62..f52715abb1 100644 --- a/src/modules/attitude_estimator_so3/module.mk +++ b/src/modules/attitude_estimator_so3/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = attitude_estimator_so3 SRCS = attitude_estimator_so3_main.cpp \ attitude_estimator_so3_params.c + +MODULE_STACKSIZE = 1200 From 4bc06381a9b44e7da22f21c20082146ad5d29c1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 08:15:59 +0200 Subject: [PATCH 111/139] commander: Cleanup properly after out-of-mem error --- src/modules/commander/mag_calibration.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9296db6edc..388820511c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -121,6 +121,20 @@ int do_mag_calibration(int mavlink_fd) if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + + /* clean up */ + if (x != NULL) { + free(x); + } + + if (y != NULL) { + free(y); + } + + if (z != NULL) { + free(z); + } + res = ERROR; return res; } From 0655aeb7ecb73eeaedfbd41171f07f9a247b32db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 08:17:31 +0200 Subject: [PATCH 112/139] startup: NuttX seems to free memory only AFTER the next command is issued, requiring us to give it some time to do memory management so it does not keep starting tasks on top of each other. May need some consideration on main startup script as well. --- ROMFS/px4fmu_common/init.d/rc.usb | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index afdba92afd..b2cd62222f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,11 +6,17 @@ 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 +usleep 1000 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +usleep 1000 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 +usleep 1000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 +usleep 1000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 +usleep 1000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 +usleep 1000 # Exit shell to make it available to MAVLink exit From 1e13b5a02c4aab5b32f43b6bf2b3588674ee84ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:02:31 +0200 Subject: [PATCH 113/139] commander: Reduce calibration count, minimally reduce stack sizes after careful inspection --- src/modules/commander/commander.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcdc..13da27dcd0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2950, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -743,7 +743,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 2900); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 388820511c..0ead22f773 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_interval = 45 * 1000 * 1000; /* maximum 500 values */ - const unsigned int calibration_maxcount = 500; + const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; struct mag_scale mscale_null = { From b2945fda53839fb70678ea6a1ff286db36ebee8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:03:05 +0200 Subject: [PATCH 114/139] param command: Reduce stack size to 1800 after careful testing --- src/systemcmds/param/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk index 63f15ad286..f716eb71e9 100644 --- a/src/systemcmds/param/module.mk +++ b/src/systemcmds/param/module.mk @@ -38,7 +38,8 @@ MODULE_COMMAND = param SRCS = param.c -MODULE_STACKSIZE = 4096 +# Note: measurements yielded a max of 900 bytes used. +MODULE_STACKSIZE = 1800 MAXOPTIMIZATION = -Os From 35e7e375de30c3ce0f830cd99ec22f5250e6d95d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:03:45 +0200 Subject: [PATCH 115/139] reboot command: Reduce stack size to 800 - it really just reboots and does not need stack --- src/systemcmds/reboot/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk index 19f64af54f..edf9d8b376 100644 --- a/src/systemcmds/reboot/module.mk +++ b/src/systemcmds/reboot/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = reboot SRCS = reboot.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 800 From 2cda6820747986c48de02be0c09d7ce67e63c6f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:04:16 +0200 Subject: [PATCH 116/139] sdlog2 start tool: Reduce stack, since it just starts the app --- src/modules/sdlog2/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f531296880..a28d43e725 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c + +MODULE_STACKSIZE = 1200 From b43f2e8be95417cdb58b670e549cffc6445b8f81 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:04:45 +0200 Subject: [PATCH 117/139] USB startup: Give NuttX enough time to tear down an app and free memory before starting the next --- ROMFS/px4fmu_common/init.d/rc.usb | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index b2cd62222f..76593881db 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,17 +6,17 @@ 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 -usleep 1000 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 -usleep 1000 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -usleep 1000 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 -usleep 1000 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 -usleep 1000 +usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 -usleep 1000 +usleep 100000 # Exit shell to make it available to MAVLink exit From ddbad698bc0708af18505e50b0429564a8cb47fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:05:21 +0200 Subject: [PATCH 118/139] mavlink start tool: Reduce stack size to 1000 - it is really just the commandline handler --- src/modules/mavlink/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 4ec6c02c83..f532e26fea 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -49,4 +49,4 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MAXOPTIMIZATION = -Os -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1024 From 9f097c1858d002eecbcda8cf8272fb8fbde1a31e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:11:32 +0200 Subject: [PATCH 119/139] top: Reduce stack size, but leave some room if it has to print a few more apps --- src/systemcmds/top/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk index 985ea846f8..548a09f856 100644 --- a/src/systemcmds/top/module.mk +++ b/src/systemcmds/top/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = top SRCS = top.c -MODULE_STACKSIZE = 2048 +MODULE_STACKSIZE = 1700 MAXOPTIMIZATION = -Os From 8e43db7bc0c1d1f7525401c3df9e0227781ab454 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 15 May 2014 10:22:42 +0200 Subject: [PATCH 120/139] mc_att_control: yaw offset limiting bug fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e74546f113..ee457c120b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -509,10 +509,10 @@ MulticopterAttitudeControl::control_attitude(float dt) float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); if (yaw_offs < - yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs); + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs); + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; publish_att_sp = true; From 899998e58fcf59f2a2e7145bef8c1972286c3c42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 10:28:48 +0200 Subject: [PATCH 121/139] px4io driver: Use modern-days syntax to start task --- src/drivers/px4io/px4io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3c3b598627..da454eb853 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -794,7 +794,12 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_spawn_cmd("px4io", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 2000, + (main_t)&PX4IO::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); From a4c4080d63696fc30577c157a9659a868c4ec111 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 14:18:38 +0200 Subject: [PATCH 122/139] Fixed alt ref init --- .../fw_att_pos_estimator_main.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0921de8690..907f4c2e1d 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1041,9 +1041,9 @@ FixedwingEstimator::task_main() // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_gps_offset = gps_alt - _baro.altitude; + _baro_gps_offset = _baro_ref - _baro.altitude; _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; @@ -1060,14 +1060,14 @@ FixedwingEstimator::task_main() // Initialize projection _local_pos.ref_lat = lat; _local_pos.ref_lon = lon; - _local_pos.ref_alt = _baro_ref + _baro_gps_offset; + _local_pos.ref_alt = gps_alt; _local_pos.ref_timestamp = _gps.timestamp_position; map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; @@ -1083,6 +1083,10 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; + + _local_pos.ref_alt = _baro_ref; + _baro_gps_offset = 0.0f; + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } } @@ -1262,7 +1266,8 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9] + _baro_gps_offset; + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -1307,8 +1312,8 @@ FixedwingEstimator::task_main() _global_pos.vel_e = 0.0f; } - /* local pos alt is negative, change sign and add alt offset */ - _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1384,6 +1389,7 @@ FixedwingEstimator::print_status() // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); From 19ebe076cdb2bf78c39e29db845c674b3b43a5c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 14:38:16 +0200 Subject: [PATCH 123/139] Fix aerocore config --- makefiles/config_aerocore_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index e038500a6a..d6902c4ee2 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -53,7 +53,7 @@ MODULES += modules/mavlink # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav # From b903fc1ed3966400b9a585fd155bf844557fc345 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 09:50:10 +0200 Subject: [PATCH 124/139] px4io: Improve the documentation of the protocol header, NO FUNCTIONAL CHANGES --- src/modules/px4iofirmware/protocol.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 91975f2a07..313084fe9d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -209,15 +209,16 @@ enum { /* DSM bind states */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* 12 occupied by CRC */ + /* storage space of 12 occupied by CRC */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state - this is a non-data write and + hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ -#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into - 'armed' (PWM enabled) state */ -#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ +#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ #define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ From 4fe0027e7a23fdd425479c98f0167adf596d5743 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 09:51:23 +0200 Subject: [PATCH 125/139] px4io driver: Only try to upload if we have a non-zero failsafe throttle value --- src/drivers/px4io/px4io.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index da454eb853..4099e55226 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -994,13 +994,17 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) { + if (failsafe_param != PARAM_INVALID) { param_get(failsafe_param, &failsafe_param_val); - uint16_t failsafe_thr = failsafe_param_val; - pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); - if (pret != OK) { - log("failsafe upload failed"); + + if (failsafe_param_val > 0) { + + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed, FS: %d us", (int)failsafe_thr); + } } } From 5ca96631fc02d3826d2ef6274f9da7e12e9fb4a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 09:52:46 +0200 Subject: [PATCH 126/139] px4io firmware: Actually accept failsafe values, check their range. Add an error return code for commands that were rejected on reboot and force safety instead of failing silently with an OK value --- src/modules/px4iofirmware/registers.c | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fd7c6081f6..a43bb65a41 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -160,6 +160,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_ALTRATE] = 200, #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#else + /* this is unused, but we will pad it to be safe */ + [PX4IO_P_SETUP_RELAYS] = 0, #endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, @@ -562,12 +565,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed - break; + return -1; } // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) - break; + return -1; // we schedule a reboot rather than rebooting // immediately to allow the IO board to ACK @@ -582,6 +585,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else { + return -1; + } + break; + + case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: + if (value > 650 && value < 2350) { + r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; + } else { + return -1; } break; From 1b491005d1192bfee7cf25068195b5f5e3a604eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:43:50 +0200 Subject: [PATCH 127/139] Minor cleanups on handling, make wrong reg accesses non-fatal for the transfer --- src/modules/px4iofirmware/protocol.h | 2 ++ src/modules/px4iofirmware/registers.c | 27 ++++++++++++++------------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 313084fe9d..7471faec7b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -190,6 +190,8 @@ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#else +#define PX4IO_P_SETUP_RELAYS_PAD 5 #endif #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a43bb65a41..db1836f4a5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -161,8 +161,8 @@ volatile uint16_t r_page_setup[] = #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, #else - /* this is unused, but we will pad it to be safe */ - [PX4IO_P_SETUP_RELAYS] = 0, + /* this is unused, but we will pad it for readability (the compiler pads it automatically) */ + [PX4IO_P_SETUP_RELAYS_PAD] = 0, #endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, @@ -526,18 +526,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 50) + if (value < 50) { value = 50; - if (value > 400) + } + if (value > 400) { value = 400; + } pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; case PX4IO_P_SETUP_PWM_ALTRATE: - if (value < 50) + if (value < 50) { value = 50; - if (value > 400) + } + if (value > 400) { value = 400; + } pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; @@ -565,12 +569,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed - return -1; + break; } // check the magic value - if (value != PX4IO_REBOOT_BL_MAGIC) - return -1; + if (value != PX4IO_REBOOT_BL_MAGIC) { + break; + } // we schedule a reboot rather than rebooting // immediately to allow the IO board to ACK @@ -585,16 +590,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - } else { - return -1; } break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; - } else { - return -1; } break; From f66cd1a81400c80166aae8a65552370bf206c11b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:44:44 +0200 Subject: [PATCH 128/139] frsky telem: reduce stack, fix missing header --- src/drivers/frsky_telemetry/frsky_data.c | 2 ++ src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/frsky_telemetry/module.mk | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 57a03bc844..18de3f4dac 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,6 +53,8 @@ #include #include +#include + /* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 7b08ca69e9..6e08390436 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[]) frsky_task = task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 2000, frsky_telemetry_thread_main, (const char **)argv); diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 1632c74f75..9a49589ee5 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = frsky_telemetry SRCS = frsky_data.c \ frsky_telemetry.c + +MODULE_STACKSIZE = 1200 From 842819a6d6984583237f0ebcbc3bd77354574cd7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:45:00 +0200 Subject: [PATCH 129/139] Use smaller logging buffer --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 009e7431ac..517e073afb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 5 -t + sdlog2 start -r 50 -a -b 4 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t From 0a6861e98ddb5f25c6564e2251bb05dc0606bfc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:45:37 +0200 Subject: [PATCH 130/139] FMUv1: Reduce user main stack slightly, still 2.5K buffer between actual use and size --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index c599b118f7..bc0c120679 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -418,7 +418,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_USERMAIN_STACKSIZE=3500 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 From 8630d82ea2b27389b686fe191e9b2f2fbd44f235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:46:10 +0200 Subject: [PATCH 131/139] gps driver: Use correct spawn command, use a smaller start tool size --- src/drivers/gps/gps.cpp | 12 ++++-------- src/drivers/gps/module.mk | 2 ++ 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2d8d37298e..6195cd6eab 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -78,12 +79,6 @@ #endif static const int ERROR = -1; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - - - class GPS : public device::CDev { public: @@ -211,7 +206,8 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_spawn_cmd("gps", SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 82c67d40a3..eb382c4b26 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -41,3 +41,5 @@ SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ ubx.cpp + +MODULE_STACKSIZE = 1200 From 9d2bfb596cd312c4de185f1932cb17f743deecd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:46:47 +0200 Subject: [PATCH 132/139] flow example: Use smaller stack and correct start logic --- .../flow_position_estimator/flow_position_estimator_main.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 495c415f2b..c775428ef1 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -65,6 +65,7 @@ #include #include #include +#include #include #include "flow_position_estimator_params.h" @@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_RR, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 4096, + 4000, flow_position_estimator_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From cccd3e1dc47968cbe5d351bc327502196988215e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:47:18 +0200 Subject: [PATCH 133/139] mavlink app: Reduce stack sizes minimally after further inspection --- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 199e85305c..6c97bfca71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2204,7 +2204,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2000, + 1950, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b03a68c076..72b9ee83a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -949,7 +949,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 3000); + pthread_attr_setstacksize(&receiveloop_attr, 2900); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); From 834a230fcf5d24cacaa16b2a0bb3add2a56ebb46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:47:41 +0200 Subject: [PATCH 134/139] inav: Reduce stack size of start tool --- src/modules/position_estimator_inav/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 939d768495..0658d3f09c 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = position_estimator_inav SRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ inertial_filter.c + +MODULE_STACKSIZE = 1200 From 30123c8f0bad29c53d1840458c90be35411237d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:48:15 +0200 Subject: [PATCH 135/139] inav: Fix scheduling type, we want ALL processes to stick to DEFAULT --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 368424853c..d8d0ff37d3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -135,7 +135,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From ea99fd84464ba6141f182dece0ae7a60c3ea9377 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 10:48:41 +0200 Subject: [PATCH 136/139] nshterm: Reduce stack size further after more thorough inspection --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index fe79ec3b76..b22b446dae 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1500 +MODULE_STACKSIZE = 1200 From f52edc4cdee0aec037d5def8ee805bb1904bb6f9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 09:18:54 +0200 Subject: [PATCH 137/139] ekf_att_pos_estimator: fixed files names and perf counters names --- ...r_main.cpp => ekf_att_pos_estimator_main.cpp} | 16 ++++++++-------- ...r_params.c => ekf_att_pos_estimator_params.c} | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_main.cpp => ekf_att_pos_estimator_main.cpp} (98%) rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_params.c => ekf_att_pos_estimator_params.c} (99%) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp similarity index 98% rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 907f4c2e1d..7a71894ed8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file fw_att_pos_estimator_main.cpp + * @file ekf_att_pos_estimator_main.cpp * Implementation of the attitude and position estimator. * * @author Paul Riseborough @@ -336,13 +336,13 @@ FixedwingEstimator::FixedwingEstimator() : _baro_gps_offset(0.0f), /* performance counters */ - _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")), + _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")), + _perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")), + _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")), /* states */ _initialized(false), diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c similarity index 99% rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index d2c6e1f15e..1d9ae46236 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file fw_att_pos_estimator_params.c + * @file ekf_att_pos_estimator_params.c * * Parameters defined by the attitude and position estimator task * From 98fcec243bac42507aa4469bd79831f847e2e846 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 09:21:47 +0200 Subject: [PATCH 138/139] ekf_att_pos_estimator: module.mk fixed --- src/modules/ekf_att_pos_estimator/module.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 30955d0ddc..6fefec2c2a 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -32,11 +32,11 @@ ############################################################################ # -# Main Attitude and Position Estimator for Fixed Wing Aircraft +# Main EKF Attitude and Position Estimator # MODULE_COMMAND = ekf_att_pos_estimator -SRCS = fw_att_pos_estimator_main.cpp \ - fw_att_pos_estimator_params.c \ +SRCS = ekf_att_pos_estimator_main.cpp \ + ekf_att_pos_estimator_params.c \ estimator.cpp From 32ae2dd1d094f4554f5acdad8fb76ed9eb3ba1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 11:00:14 +0200 Subject: [PATCH 139/139] commander: missed 'break' in 'switch' added --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd0..504696ff94 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1710,6 +1710,7 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break;