From 60706500df4e5264f77d0a7c36edb7b6b60e21ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:48:38 +0200 Subject: [PATCH 01/64] Make some space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f3..97eddfdd2b 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) From 171857811f76fb4d8fb3b65329345dd5af0ec9db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:49:46 +0200 Subject: [PATCH 02/64] EKF filter update. Now correctly scaling variance (well, if you could call this ever "correct") for repeated fusion of the same data quantity. --- .../ekf_att_pos_estimator_main.cpp | 33 +++++++-- .../estimator_23states.cpp | 72 +++++++++++++++---- .../estimator_23states.h | 17 ++++- 3 files changed, 104 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 91d17e787c..6cb68fe779 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -96,7 +96,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); +__EXPORT uint64_t getMicros(); + static uint64_t IMUmsec = 0; +static uint64_t IMUusec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; uint32_t millis() @@ -104,6 +107,11 @@ uint32_t millis() return IMUmsec; } +uint64_t getMicros() +{ + return IMUusec; +} + class FixedwingEstimator { public: @@ -850,7 +858,8 @@ FixedwingEstimator::task_main() } _last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3f; + IMUmsec = _gyro.timestamp / 1e3; + IMUusec = _gyro.timestamp; float deltaT = (_gyro.timestamp - _last_run) / 1e6f; _last_run = _gyro.timestamp; @@ -914,7 +923,8 @@ FixedwingEstimator::task_main() // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3f; + IMUmsec = _sensor_combined.timestamp / 1e3; + IMUusec = _sensor_combined.timestamp; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; @@ -994,8 +1004,6 @@ FixedwingEstimator::task_main() if (gps_updated) { - last_gps = _gps.timestamp_position; - orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1008,11 +1016,17 @@ FixedwingEstimator::task_main() _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) { + float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + + const float pos_reset_threshold = 5.0f; // seconds + + /* timeout of 5 seconds */ + if (gps_elapsed > pos_reset_threshold) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); /* fuse GPS updates */ @@ -1044,6 +1058,8 @@ FixedwingEstimator::task_main() newDataGps = true; + last_gps = _gps.timestamp_position; + } } @@ -1052,8 +1068,15 @@ FixedwingEstimator::task_main() orb_check(_baro_sub, &baro_updated); if (baro_updated) { + + hrt_abstime baro_last = _baro.timestamp; + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1)); + _ekf->baroHgt = _baro.altitude; if (!_baro_init) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c7c7305b2a..d1349535d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2,6 +2,11 @@ #include #include #include +#include + +#ifndef M_PI_F +#define M_PI_F ((float)M_PI) +#endif #define EKF_COVARIANCE_DIVERGED 1.0e8f @@ -38,6 +43,7 @@ AttPosEKF::AttPosEKF() : resetStates{}, storedStates{}, statetimeStamp{}, + lastVelPosFusion(millis()), statesAtVelTime{}, statesAtPosTime{}, statesAtHgtTime{}, @@ -59,7 +65,12 @@ AttPosEKF::AttPosEKF() : accel(), dVelIMU(), dAngIMU(), - dtIMU(0), + dtIMU(0.005f), + dtIMUfilt(0.005f), + dtVelPos(0.01f), + dtVelPosFilt(0.01f), + dtHgtFilt(0.01f), + dtGpsFilt(0.1f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -260,6 +271,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Constrain states (to protect against filter divergence) ConstrainStates(); + + // update filtered IMU time step length + dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU; } void AttPosEKF::CovariancePrediction(float dt) @@ -962,6 +976,21 @@ void AttPosEKF::CovariancePrediction(float dt) ConstrainVariances(); } +void AttPosEKF::updateDtGpsFilt(float dt) +{ + dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f; +} + +void AttPosEKF::updateDtHgtFilt(float dt) +{ + dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f; +} + +void AttPosEKF::updateDtVelPosFilt(float dt) +{ + dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f; +} + void AttPosEKF::FuseVelposNED() { @@ -998,6 +1027,18 @@ void AttPosEKF::FuseVelposNED() // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { + uint64_t tNow = getMicros(); + updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f); + lastVelPosFusion = tNow; + + // scaler according to the number of repetitions of the + // same measurement in one fusion step + float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt; + + // scaler according to the number of repetitions of the + // same measurement in one fusion step + float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt; + // set the GPS data timeout depending on whether airspeed data is present if (useAirspeed) horizRetryTime = gpsRetryTime; else horizRetryTime = gpsRetryTimeNoTAS; @@ -1010,12 +1051,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] = sq(vneSigma) + sq(velErr); + R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = sq(vdSigma) + sq(velErr); - R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr); + R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr); R_OBS[4] = R_OBS[3]; - R_OBS[5] = sq(posDSigma) + sq(posErr); + R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr); // calculate innovations and check GPS data validity using an innovation consistency check if (fuseVelData) @@ -1995,9 +2036,8 @@ void AttPosEKF::FuseOptFlow() varInnovOptFlow[0] = 1.0f/SK_LOS[0]; innovOptFlow[0] = losPred[0] - losData[0]; - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; + // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag + obsIndex = 1; fuseOptFlowData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement @@ -2041,6 +2081,10 @@ void AttPosEKF::FuseOptFlow() Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); varInnovOptFlow[1] = 1.0f/SK_LOS[1]; innovOptFlow[1] = losPred[1] - losData[1]; + + // reset the observation index + obsIndex = 0; + fuseOptFlowData = false; } // Check the innovation for consistency and don't fuse if > 3Sigma @@ -2102,10 +2146,9 @@ void AttPosEKF::FuseOptFlow() P[i][j] = P[i][j] - KHP[i][j]; } } + ForceSymmetry(); + ConstrainVariances(); } - obsIndex = obsIndex + 1; - ForceSymmetry(); - ConstrainVariances(); } void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -3006,9 +3049,14 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - + dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f); + dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f); + dtGpsFilt = 1.0f / 5.0f; + dtHgtFilt = 1.0f / 100.0f; storeIndex = 0; + lastVelPosFusion = millis(); + // Do the data structure init for (unsigned i = 0; i < n_states; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index faa6735caa..12cbc53b8d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -116,6 +116,9 @@ public: float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + // Times + uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement @@ -140,7 +143,12 @@ public: Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter + float dtIMUfilt; // average time between IMU measurements (sec) + float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter + float dtVelPosFilt; // average time between position / velocity fusion steps + float dtHgtFilt; // average time between height measurement updates + float dtGpsFilt; // average time between gps measurement updates uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -211,6 +219,9 @@ public: unsigned storeIndex; +void updateDtGpsFilt(float dt); + +void updateDtHgtFilt(float dt); void UpdateStrapdownEquationsNED(); @@ -300,6 +311,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination); protected: +void updateDtVelPosFilt(float dt); + bool FilterHealthy(); bool GyroOffsetsDiverged(); @@ -314,3 +327,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl uint32_t millis(); +uint64_t getMicros(); + From cfcb76f627d164b3244e9b81b2fe2fe2e54ee045 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 21:23:59 +0200 Subject: [PATCH 03/64] Add long-term stable wind estimate --- .../estimator_23states.cpp | 21 +++++++++++++++++++ .../estimator_23states.h | 3 +++ 2 files changed, 24 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d1349535d4..d176ccee21 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -71,6 +71,9 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), + windSpdFiltNorth(0.0f), + windSpdFiltEast(0.0f), + windSpdFiltAltitude(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -1657,6 +1660,24 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { + // Lowpass the output of the wind estimate - we want a long-term + // stable estimate, but not start to load into the overall dynamics + // of the system (which adjusting covariances would do) + + // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations + // assuming equal wind speeds on the same altitude and varying wind speeds on + // different altitudes + float windFiltCoeff = 0.0002f; + + float altDiff = fabsf(windSpdFiltAltitude - hgtMea); + + // Change filter coefficient based on altitude + windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f); + + windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); + windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); + windSpdFiltAltitude = hgtMea; + // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 12cbc53b8d..6349b03f02 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -149,6 +149,9 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates + float windSpdFiltNorth; // average wind speed north component + float windSpdFiltEast; // average wind speed east component + float windSpdFiltAltitude; // the last altitude used to filter wind speed uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output From 2c0d192944744086905e622f445a523d6650cdc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 21:26:18 +0200 Subject: [PATCH 04/64] Use the wind speed estimate filtered values --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 6cb68fe779..c384b25661 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1490,8 +1490,10 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; + _wind.windspeed_north = _ekf->windSpdFiltNorth; + _wind.windspeed_east = _ekf->windSpdFiltEast; + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; From 708ee8ae3aa4d4d5eac9e2fd86ed9673c126fe33 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Aug 2014 20:30:06 +0200 Subject: [PATCH 05/64] autolaunch: added param for delay --- .../launchdetection/CatapultLaunchMethod.cpp | 19 ++++++++++++++++--- .../launchdetection/CatapultLaunchMethod.h | 4 ++++ .../launchdetection/launchdetection_params.c | 9 +++++++++ 3 files changed, 29 insertions(+), 3 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a692..c4425bbe09 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -51,7 +51,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : integrator(0.0f), launchDetected(false), threshold_accel(this, "A"), - threshold_time(this, "T") + threshold_time(this, "T"), + delay(this, "DELAY") { } @@ -78,21 +79,33 @@ void CatapultLaunchMethod::update(float accel_x) // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); /* reset integrator */ integrator = 0.0f; - launchDetected = false; + } + + if (launchDetected) { + delayCounter += dt; + if (delayCounter > delay.get()) { + delayPassed = true; + } } } bool CatapultLaunchMethod::getLaunchDetected() { - return launchDetected; + if (delay.get() > 0.0f) { + return delayPassed; + } else { + return launchDetected; + } } void CatapultLaunchMethod::reset() { integrator = 0.0f; + delayCounter = 0.0f; launchDetected = false; + delayPassed = false; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 23757f6f37..a64d482f6a 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -63,10 +63,14 @@ public: private: hrt_abstime last_timestamp; float integrator; + float delayCounter; + bool launchDetected; + bool delayPassed; control::BlockParamFloat threshold_accel; control::BlockParamFloat threshold_time; + control::BlockParamFloat delay; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 8df8c696c2..02f01bc667 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -77,6 +77,15 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); +/** + * Catapult delay + * + * + * + * @min 0 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f); /** * Throttle setting while detecting launch. * From 5a5617cb597b16b789a6fa175410d667f45cf907 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:42:53 +0200 Subject: [PATCH 06/64] launchdetector: return state of detection The launchdetector now has a intermediate state (controlsenabled) which is meant to be interpreted by the controller as: "perform attitude control but do not yet power up the motor". This can be used in the floating phase during a bungee launch for example. --- .../launchdetection/CatapultLaunchMethod.cpp | 70 +++++++++++-------- .../launchdetection/CatapultLaunchMethod.h | 13 ++-- src/lib/launchdetection/LaunchDetector.cpp | 27 +++++-- src/lib/launchdetection/LaunchDetector.h | 8 ++- src/lib/launchdetection/LaunchMethod.h | 13 +++- .../launchdetection/launchdetection_params.c | 8 ++- 6 files changed, 91 insertions(+), 48 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 2b876b6291..65ae461dbc 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -49,10 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), - launchDetected(false), - threshold_accel(this, "A"), - threshold_time(this, "T"), - delay(this, "DELAY") + state(LAUNCHDETECTION_RES_NONE), + thresholdAccel(this, "A"), + thresholdTime(this, "T"), + motorDelay(this, "MDEL") { } @@ -66,46 +66,56 @@ void CatapultLaunchMethod::update(float accel_x) float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; last_timestamp = hrt_absolute_time(); - if (accel_x > threshold_accel.get()) { - integrator += dt; -// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_time.get()) { - launchDetected = true; + switch (state) { + case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > thresholdAccel.get()) { + integrator += dt; + if (integrator > thresholdTime.get()) { + if (motorDelay.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" + " throttle", (double)motorDelay.get()); + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + warnx("Launch detected: state: enablemotors (delay not activated)"); + } + } + } else { + /* reset */ + reset(); } + break; - } else { -// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - /* reset integrator */ - integrator = 0.0f; - } - - if (launchDetected) { - delayCounter += dt; - if (delayCounter > delay.get()) { - delayPassed = true; + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + * over to allow full throttle */ + motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { + warnx("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + break; + + default: + break; + } } -bool CatapultLaunchMethod::getLaunchDetected() +LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const { - if (delay.get() > 0.0f) { - return delayPassed; - } else { - return launchDetected; - } + return state; } void CatapultLaunchMethod::reset() { integrator = 0.0f; - delayCounter = 0.0f; - launchDetected = false; - delayPassed = false; + motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index a64d482f6a..d918c3a763 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -57,20 +57,19 @@ public: ~CatapultLaunchMethod(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected() const; void reset(); private: hrt_abstime last_timestamp; float integrator; - float delayCounter; + float motorDelayCounter; - bool launchDetected; - bool delayPassed; + LaunchDetectionResult state; - control::BlockParamFloat threshold_accel; - control::BlockParamFloat threshold_time; - control::BlockParamFloat delay; + control::BlockParamFloat thresholdAccel; + control::BlockParamFloat thresholdTime; + control::BlockParamFloat motorDelay; }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 3bf47bbb03..2958c0a315 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -46,6 +46,7 @@ namespace launchdetection LaunchDetector::LaunchDetector() : SuperBlock(NULL, "LAUN"), + activeLaunchDetectionMethodIndex(-1), launchdetection_on(this, "ALL_ON"), throttlePreTakeoff(this, "THR_PRE") { @@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - launchMethods[0]->reset(); + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->reset(); + } + + /* Reset active launchdetector */ + activeLaunchDetectionMethodIndex = -1; + + } void LaunchDetector::update(float accel_x) @@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x) } } -bool LaunchDetector::getLaunchDetected() +LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected()) { - return true; + if (activeLaunchDetectionMethodIndex < 0) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + warnx("selecting launchmethod %d", i); + activeLaunchDetectionMethodIndex = i; // from now on only check this method + return launchMethods[i]->getLaunchDetected(); + } } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } } - return false; + return LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 1a214b66ee..b48e724bad 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,7 +59,7 @@ public: void reset(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -67,6 +67,12 @@ public: // virtual bool getLaunchDetected(); protected: private: + int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods + which detected a Launch. If no launchMethod has detected a launch yet the + value is -1. Once one launchMetthod has detected a launch only this + method is checked for further adavancing in the state machine (e.g. when + to power up the motors) */ + LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index e04467f6a4..d2f091cea7 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -44,11 +44,22 @@ namespace launchdetection { +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up and still be set to 'throttlePreTakeoff'. + For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control + attitude and also throttle up the motors. */ +}; + class LaunchMethod { public: virtual void update(float accel_x) = 0; - virtual bool getLaunchDetected() = 0; + virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; protected: diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 02f01bc667..d35eb11f60 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -78,14 +78,16 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** - * Catapult delay - * + * Motor delay * + * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * + * @unit seconds * @min 0 * @group Launch detection */ -PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); /** * Throttle setting while detecting launch. * From 40d47fb069cb60303102f3bfba21f6dc5e0aa5a9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:45:20 +0200 Subject: [PATCH 07/64] fw pos control: use new lauchdetector states --- .../fw_pos_control_l1_main.cpp | 73 +++++++++++-------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 350ce6dec1..6dd4585162 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +using namespace launchdetection; + class FixedwingPositionControl { public: @@ -171,7 +173,7 @@ private: bool land_onslope; /* takeoff/launch states */ - bool launch_detected; + LaunchDetectionResult launch_detection_state; bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -438,7 +440,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - launch_detected(false), + launch_detection_state(LAUNCHDETECTION_RES_NONE), usePreTakeoffThrust(false), last_manual(false), landingslope(), @@ -1076,39 +1078,51 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ - if(!launch_detected) { //do not do further checks once a launch was detected - if (launchDetector.launchDetectionEnabled()) { - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - last_sent = hrt_absolute_time(); - } + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + last_sent = hrt_absolute_time(); + } + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the laucn detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + + /* Depending on launch detection state set control flags */ + if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { /* Tell the attitude controller to stop integrating while we are waiting * for the launch */ _att_sp.roll_reset_integral = true; _att_sp.pitch_reset_integral = true; _att_sp.yaw_reset_integral = true; - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - if (launchDetector.getLaunchDetected()) { - launch_detected = true; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); - } - } else { - /* no takeoff detection --> fly */ - launch_detected = true; - warnx("launchdetection off"); + usePreTakeoffThrust = true; + } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { + usePreTakeoffThrust = true; + } else { + usePreTakeoffThrust = false; } + } else { + /* no takeoff detection --> fly */ + usePreTakeoffThrust = false; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. The usePreTakeoffThrust + * flag determines how much thrust we apply */ - if (launch_detected) { - usePreTakeoffThrust = false; + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() : + _parameters.throttle_max; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { @@ -1119,7 +1133,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), @@ -1138,17 +1152,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + takeoff_throttle, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } - - } else { - usePreTakeoffThrust = true; } + } /* reset landing state */ @@ -1182,6 +1194,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* making sure again that the correct thrust is used, without depending on library calls */ if (usePreTakeoffThrust) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } @@ -1342,7 +1355,7 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detected = false; + launch_detection_state = LAUNCHDETECTION_RES_NONE; usePreTakeoffThrust = false; launchDetector.reset(); } From 9f5004be2d282a0bb8f2618545d0c6174df2e29a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:13:30 +0200 Subject: [PATCH 08/64] fw pos control: set default roll, pitch while waiting for launch --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6dd4585162..9cbe5483bf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1143,7 +1143,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1159,6 +1160,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt, ground_speed); } + } else { + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); } } From cfbbe60291028c60a5dd0a8f2b8bad265a7ee839 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:41:48 +0200 Subject: [PATCH 09/64] fw pos control: launchdetection logic cleanup --- .../fw_pos_control_l1_main.cpp | 44 ++++++++----------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9cbe5483bf..da81b0dba0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -174,7 +174,6 @@ private: /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; - bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -441,7 +440,6 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detection_state(LAUNCHDETECTION_RES_NONE), - usePreTakeoffThrust(false), last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), @@ -1092,37 +1090,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update our copy of the laucn detection state */ launch_detection_state = launchDetector.getLaunchDetected(); - - /* Depending on launch detection state set control flags */ - if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - - usePreTakeoffThrust = true; - } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { - usePreTakeoffThrust = true; - } else { - usePreTakeoffThrust = false; - } } else { /* no takeoff detection --> fly */ - usePreTakeoffThrust = false; - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + /* Set control values depending on the detection state */ if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. The usePreTakeoffThrust - * flag determines how much thrust we apply */ + /* Launch has been detected, hence we have to control the plane. */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() : - _parameters.throttle_max; + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { @@ -1161,6 +1146,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), @@ -1200,8 +1191,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - /* making sure again that the correct thrust is used, without depending on library calls */ - if (usePreTakeoffThrust) { + /* Copy thrust and pitch values from tecs + * making sure again that the correct thrust is used, without depending on library calls */ + if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { @@ -1362,7 +1355,6 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; - usePreTakeoffThrust = false; launchDetector.reset(); } From bcca3cae748ffea2df51907992e4a3c7ca673fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Aug 2014 16:03:24 +0200 Subject: [PATCH 10/64] Run full update straight after reset, filter wind speed dynamically --- .../ekf_att_pos_estimator_main.cpp | 22 +--------- .../estimator_23states.cpp | 40 +++++++++++++------ .../estimator_23states.h | 1 + 3 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c384b25661..b7e118d47e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1220,30 +1220,12 @@ FixedwingEstimator::task_main() } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } + // check (and reset the filter as needed) + (void)check_filter_state(); // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); - #if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - - quat2eul(eulerEst, tempQuat); - - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - - #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d176ccee21..249cc419ed 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -74,6 +74,7 @@ AttPosEKF::AttPosEKF() : windSpdFiltNorth(0.0f), windSpdFiltEast(0.0f), windSpdFiltAltitude(0.0f), + windSpdFiltClimb(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -1660,22 +1661,30 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { - // Lowpass the output of the wind estimate - we want a long-term - // stable estimate, but not start to load into the overall dynamics - // of the system (which adjusting covariances would do) - - // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations - // assuming equal wind speeds on the same altitude and varying wind speeds on - // different altitudes - float windFiltCoeff = 0.0002f; float altDiff = fabsf(windSpdFiltAltitude - hgtMea); - // Change filter coefficient based on altitude - windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f); + if (isfinite(windSpdFiltClimb)) { + windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]); + } else { + windSpdFiltClimb = states[6]; + } + + if (altDiff < 20.0f) { + // Lowpass the output of the wind estimate - we want a long-term + // stable estimate, but not start to load into the overall dynamics + // of the system (which adjusting covariances would do) + + // Change filter coefficient based on altitude change rate + float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f); + + windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); + windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); + } else { + windSpdFiltNorth = vwn; + windSpdFiltEast = vwe; + } - windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); - windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); windSpdFiltAltitude = hgtMea; // Calculate observation jacobians @@ -3097,6 +3106,13 @@ void AttPosEKF::ZeroVariables() dVelIMU.zero(); lastGyroOffset.zero(); + windSpdFiltNorth = 0.0f; + windSpdFiltEast = 0.0f; + // setting the altitude to zero will give us a higher + // gain to adjust faster in the first step + windSpdFiltAltitude = 0.0f; + windSpdFiltClimb = 0.0f; + 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/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 6349b03f02..f8bb7a9c42 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -152,6 +152,7 @@ public: float windSpdFiltNorth; // average wind speed north component float windSpdFiltEast; // average wind speed east component float windSpdFiltAltitude; // the last altitude used to filter wind speed + float windSpdFiltClimb; // filtered climb rate uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output From 92cc44fe1e09f426087e91fff88effde04b32c5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 12:07:59 +0200 Subject: [PATCH 11/64] avoid changing the reset logic --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index b7e118d47e..8fdc40d410 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1221,7 +1221,12 @@ FixedwingEstimator::task_main() // We're apparently initialized in this case now // check (and reset the filter as needed) - (void)check_filter_state(); + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); From c591444c1e2e4124d28a03653c0ccdbcd305c1c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 12:48:14 +0200 Subject: [PATCH 12/64] fw pos control: set pitch sp correctly while waiting for launch --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index da81b0dba0..b3a78ad827 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1192,7 +1192,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* Copy thrust and pitch values from tecs - * making sure again that the correct thrust is used, without depending on library calls */ + * making sure again that the correct thrust is used, + * without depending on library calls */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); @@ -1200,7 +1201,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + /* During a takeoff waypoint while waiting for launch the pitch sp is set + * already (not by tecs) */ + if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + } if (_control_mode.flag_control_position_enabled) { last_manual = false; From ccdfab245bac02565521f2d76e604f54ba3f5bd5 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:26:47 +0200 Subject: [PATCH 13/64] mixer_multirotor: topic for motor limit notification --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/multirotor_motor_limits.h | 69 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f87374..b921865ebe 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/multirotor_motor_limits.h" +ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); + #include "topics/telemetry_status.h" ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 0000000000..44c51e4d98 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif From 5aafa1b021a80868a3b5aebde13cd54d407e6e41 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:36:02 +0200 Subject: [PATCH 14/64] mixer_multirotor: motor limit notification for PX4FMU --- src/modules/systemlib/mixer/mixer.h | 6 +++++ .../systemlib/mixer/mixer_multirotor.cpp | 22 ++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc7..17989558e6 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include #include "drivers/drv_mixer.h" + +#include + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4b22a46d0b..57e17b67d2 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include +#include #include #include @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } From c79bc7073eb65f08b724a3893d183d20781f06e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:34 +0200 Subject: [PATCH 15/64] Support for termination failsafe in PWM out driver --- src/drivers/drv_pwm_output.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5aff6825be..bc586f3950 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + +/** make failsafe non-recoverable (termination) if it occurs */ +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /* * From 1fbdca4ee988d5816eebbd9fef95ce498bacfd14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:53 +0200 Subject: [PATCH 16/64] Add command to run termination failsafe --- src/systemcmds/pwm/pwm.c | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c8d698b86a..478c2a772f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[]) } } exit(0); + } else if (!strcmp(argv[1], "terminatefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); + } + + if (ret != OK) { + warnx("FAILED setting termination failsafe %s", argv[2]); + } + } + exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); + usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail"); return 0; } From 49846d476f77290093c24097e05d5a8d60d1a4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:12 +0200 Subject: [PATCH 17/64] IO firmware supports termination failsafe --- src/modules/px4iofirmware/mixer.cpp | 10 ++++++++++ src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 18 ++++++++++++++++-- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 606c639f94..1eacda97a6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,6 +154,16 @@ mixer_tick(void) } } + /* + * Check if failsafe termination is set - if yes, + * set the force failsafe flag once entering the first + * failsafe condition. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + (source == MIX_FAILSAFE)) { + r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + /* * Check if we should force failsafe - and do it if we have to */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0507836871..eae7f9567a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -180,6 +180,7 @@ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0da778b6f6..7a5a5e4846 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; From a7109609ecfeba0a11121c1b83a46b1463f55931 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:28 +0200 Subject: [PATCH 18/64] support termination failsafe in IO driver --- src/drivers/px4io/px4io.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 97919538ff..3fdb1dd1f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ + if (arg == 0) { + /* clear termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { + /* set termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ From 91d50301c61cf495e83cab59621ef83cff24da3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 10:46:10 +0200 Subject: [PATCH 19/64] Do not enter RC override if FMU is lost and termination failsafe mode requested --- src/modules/px4iofirmware/mixer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1eacda97a6..0c65b76425 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -139,7 +139,9 @@ mixer_tick(void) (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; From c85d7aae06b49f43e32dde05c00bcee1310aee9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 20:36:59 +0200 Subject: [PATCH 20/64] Make sure we got to valid input at least once before kicking in failsafe --- src/modules/px4iofirmware/mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0c65b76425..17751bd6d2 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -162,7 +163,8 @@ mixer_tick(void) * failsafe condition. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && - (source == MIX_FAILSAFE)) { + (source == MIX_FAILSAFE) && + input_valid_initialized) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -180,6 +182,9 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + + /* we got valid input, kick off the full failsafe checks */ + input_valid_initialized = true; } /* From 6773eb988017063366dce95bbfec8fae1f1913f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 21:39:33 +0200 Subject: [PATCH 21/64] Introduce FMU initialised status flag, only run failsafe trap if initialized once --- src/modules/px4iofirmware/mixer.cpp | 67 +++++++++++++++------------- src/modules/px4iofirmware/protocol.h | 1 + 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 17751bd6d2..3e19333d8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -58,7 +58,7 @@ extern "C" { /* * Maximum interval in us before FMU signal is considered lost */ -#define FMU_INPUT_DROP_LIMIT_US 200000 +#define FMU_INPUT_DROP_LIMIT_US 500000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -71,7 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -99,7 +98,8 @@ mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + if ((system_state.fmu_data_received_time == 0) || + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { @@ -110,6 +110,9 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + + /* this flag is never cleared once OK */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ @@ -157,14 +160,41 @@ mixer_tick(void) } } + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + if ( /* if we have requested flight termination style failsafe (noreturn) */ + (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && - input_valid_initialized) { + /* and we should be armed, so we intended to provide outputs */ + should_arm && + /* and FMU is initialized */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -182,35 +212,8 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); - - /* we got valid input, kick off the full failsafe checks */ - input_valid_initialized = true; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - /* * Run the mixers. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index eae7f9567a..4739f6e400 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,6 +111,7 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ From 9f94e4ac8409dc15484ee3bcfb89958890a305e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:33:59 +0200 Subject: [PATCH 22/64] add terrain alt field to global pos topic --- src/modules/uORB/topics/vehicle_global_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 25137c1c64..6cbe25fbd9 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,6 +72,7 @@ struct vehicle_global_position_s { float yaw; /**< Yaw in radians -PI..+PI. */ float eph; /**< Standard deviation of position estimate horizontally */ float epv; /**< Standard deviation of position vertically */ + float terrain_alt; /**< Terrain altitude in m, WGS84 */ }; /** From 0f548ab88cb36be362d14f9e9e76c53e3764e3c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:38:40 +0200 Subject: [PATCH 23/64] add global pos valid flag --- src/modules/uORB/topics/vehicle_global_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 6cbe25fbd9..c3bb3b8936 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -73,6 +73,7 @@ struct vehicle_global_position_s { float eph; /**< Standard deviation of position estimate horizontally */ float epv; /**< Standard deviation of position vertically */ float terrain_alt; /**< Terrain altitude in m, WGS84 */ + bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ }; /** From 1f951b0df9ad475349ba4c7e815483445128cc2a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:42:56 +0200 Subject: [PATCH 24/64] add logging for gpos terrain alt --- src/modules/sdlog2/sdlog2.c | 7 ++++++- src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index dbda8ea6f3..e135930778 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1432,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; log_msg.body.log_GPOS.eph = buf.global_pos.eph; log_msg.body.log_GPOS.epv = buf.global_pos.epv; + if (buf.global_pos.terrain_alt_valid) { + log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt; + } else { + log_msg.body.log_GPOS.terrain_alt = -1.0f; + } LOGBUFFER_WRITE_AND_COUNT(GPOS); } @@ -1464,7 +1469,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } - + /* --- VISION POSITION --- */ if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 6741c7e258..d2845eb753 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -220,6 +220,7 @@ struct log_GPOS_s { float vel_d; float eph; float epv; + float terrain_alt; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -449,7 +450,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), + LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), From 71a2025f7d904c77b7f6351ccc69acd181a42a55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 11:35:41 +0200 Subject: [PATCH 25/64] Add filter estimates --- .../ekf_att_pos_estimator_main.cpp | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 91d17e787c..e34cce078e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #ifdef SENSOR_COMBINED_SUB #include #endif @@ -171,6 +172,7 @@ private: #else int _sensor_combined_sub; #endif + int _distance_sub; /**< distance measurement */ int _airspeed_sub; /**< airspeed subscription */ int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ @@ -196,7 +198,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ - struct wind_estimate_s _wind; /**< Wind estimate */ + struct wind_estimate_s _wind; /**< wind estimate */ + struct range_finder_report _distance; /**< distance estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -226,6 +229,7 @@ private: hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; hrt_abstime _last_run; + hrt_abstime _distance_last_valid; bool _gyro_valid; bool _accel_valid; bool _mag_valid; @@ -342,6 +346,7 @@ FixedwingEstimator::FixedwingEstimator() : #else _sensor_combined_sub(-1), #endif + _distance_sub(-1), _airspeed_sub(-1), _baro_sub(-1), _gps_sub(-1), @@ -399,6 +404,7 @@ FixedwingEstimator::FixedwingEstimator() : _filter_start_time(0), _last_sensor_timestamp(0), _last_run(0), + _distance_last_valid(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), @@ -549,6 +555,7 @@ FixedwingEstimator::parameters_update() _ekf->gyroProcessNoise = _parameters.gyro_pnoise; _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF } return OK; @@ -704,6 +711,7 @@ FixedwingEstimator::task_main() /* * do subscriptions */ + _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -753,6 +761,7 @@ FixedwingEstimator::task_main() bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; + bool newRangeData = false; float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) @@ -1114,6 +1123,19 @@ FixedwingEstimator::task_main() newDataMag = false; } + orb_check(_distance_sub, &newRangeData); + + if (newRangeData) { + orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + + if (_distance.valid) { + _ekf->rngMea = _distance.distance; + _distance_last_valid = _distance.timestamp; + } else { + newRangeData = false; + } + } + /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ @@ -1334,6 +1356,13 @@ FixedwingEstimator::task_main() _ekf->fuseVtasData = false; } + if (newRangeData) { + _ekf->fuseRngData = true; + _ekf->useRangeFinder = true; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->FuseRangeFinder(); + } + // Output results math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); @@ -1447,6 +1476,10 @@ FixedwingEstimator::task_main() _global_pos.vel_d = _local_pos.vz; } + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->states[22]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; From 4c7cc10b6294c799798d05faad688d9dec2102f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 2 Sep 2014 15:37:18 +0200 Subject: [PATCH 26/64] Working and replay-tested altitude fusion --- .../ekf_att_pos_estimator_main.cpp | 4 +- .../estimator_23states.cpp | 713 ++++++++++-------- .../estimator_23states.h | 40 +- 3 files changed, 458 insertions(+), 299 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 87a20a7754..97abb76a9c 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1370,7 +1370,7 @@ FixedwingEstimator::task_main() _ekf->fuseRngData = true; _ekf->useRangeFinder = true; _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); - _ekf->FuseRangeFinder(); + _ekf->GroundEKF(); } @@ -1487,7 +1487,7 @@ FixedwingEstimator::task_main() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->states[22]; + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 249cc419ed..94a6d165fe 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -50,7 +50,7 @@ AttPosEKF::AttPosEKF() : statesAtMagMeasTime{}, statesAtVtasMeasTime{}, statesAtRngTime{}, - statesAtOptFlowTime{}, + statesAtFlowTime{}, correctedDelAng(), correctedDelVel(), summedDelAng(), @@ -118,13 +118,13 @@ AttPosEKF::AttPosEKF() : inhibitWindStates(true), inhibitMagStates(true), - inhibitGndHgtState(true), + inhibitGndState(true), onGround(true), staticMode(true), useAirspeed(true), useCompass(true), - useRangeFinder(false), + useRangeFinder(true), useOpticalFlow(false), ekfDiverged(false), @@ -132,7 +132,24 @@ AttPosEKF::AttPosEKF() : current_ekf_state{}, last_ekf_error{}, numericalProtection(true), - storeIndex(0) + storeIndex(0), + storedOmega{}, + Popt{}, + flowStates{}, + prevPosN(0.0f), + prevPosE(0.0f), + auxFlowObsInnov{}, + auxFlowObsInnovVar{}, + fScaleFactorVar(0.0f), + Tnb_flow{}, + R_LOS(0.0f), + auxFlowTestRatio{}, + auxRngTestRatio(0.0f), + flowInnovGate(0.0f), + auxFlowInnovGate(0.0f), + rngInnovGate(0.0f), + minFlowRng(0.0f), + moCompR_LOS(0.0f) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); @@ -330,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt) } else { for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; } - if (!inhibitGndHgtState) { + if (!inhibitGndState) { processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; } else { processNoise[22] = 0; @@ -1218,7 +1235,7 @@ void AttPosEKF::FuseVelposNED() } } // Don't update terrain state if inhibited - if (inhibitGndHgtState) { + if (inhibitGndState) { Kfusion[22] = 0; } @@ -1401,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[i] = 0; } } - if (!inhibitGndHgtState) { + if (!inhibitGndState) { 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]); } else { Kfusion[22] = 0; @@ -1475,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = 0; Kfusion[21] = 0; } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[1] = 1.0f/SK_MY[0]; innovMag[1] = MagPred[1] - magData.y; } @@ -1550,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = 0; Kfusion[21] = 0; } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; @@ -1746,11 +1753,6 @@ void AttPosEKF::FuseAirspeed() Kfusion[i] = 0; } } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); - } else { - Kfusion[22] = 0; - } varInnovVtas = 1.0f/SK_TAS; // Calculate the measurement innovation @@ -1882,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder() rngPred = (ptd - pd)/cosRngTilt; innovRng = rngPred - rngMea; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovRng*innovRng*SK_RNG[0]) < 25) + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); + + // Check the innovation for consistency and don't fuse if out of bounds + if (auxRngTestRatio < 1.0f) { // correct the state vector states[22] = states[22] - Kfusion[22] * innovRng; @@ -1898,286 +1903,386 @@ void AttPosEKF::FuseRangeFinder() void AttPosEKF::FuseOptFlow() { - static uint8_t obsIndex; - static float SH_LOS[13]; - static float SKK_LOS[15]; - static float SK_LOS[2]; - 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; - static float losPred[2]; +// static uint8_t obsIndex; +// static float SH_LOS[13]; +// static float SKK_LOS[15]; +// static float SK_LOS[2]; +// 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; +// static float losPred[2]; - // Transformation matrix from nav to body axes - Mat3f Tnb_local; - // Transformation matrix from body to sensor 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 Tbs; - Tbs.x.y = a3; - Tbs.y.x = -a3; - Tbs.x.z = -a2; - Tbs.z.x = a2; - Tbs.y.z = a1; - Tbs.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; +// // Transformation matrix from nav to body axes +// Mat3f Tnb_local; +// // Transformation matrix from body to sensor 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 Tbs; +// Tbs.x.y = a3; +// Tbs.y.x = -a3; +// Tbs.x.z = -a2; +// Tbs.z.x = a2; +// Tbs.y.z = a1; +// Tbs.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_local; +// Vector3f relVelSensor; + +// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. +// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) +// { +// // Sequential fusion of XY components to spread processing load across +// // two prediction time steps. + +// // Calculate observation jacobians and Kalman gains +// if (fuseOptFlowData) +// { +// // Copy required states to local variable names +// q0 = statesAtOptFlowTime[0]; +// q1 = statesAtOptFlowTime[1]; +// q2 = statesAtOptFlowTime[2]; +// q3 = statesAtOptFlowTime[3]; +// vn = statesAtOptFlowTime[4]; +// ve = statesAtOptFlowTime[5]; +// vd = statesAtOptFlowTime[6]; +// pd = statesAtOptFlowTime[9]; +// ptd = statesAtOptFlowTime[22]; +// velNED_local.x = vn; +// velNED_local.y = ve; +// velNED_local.z = vd; + +// // calculate rotation from NED to body axes +// float q00 = sq(q0); +// float q11 = sq(q1); +// float q22 = sq(q2); +// float q33 = sq(q3); +// float q01 = q0 * q1; +// float q02 = q0 * q2; +// float q03 = q0 * q3; +// float q12 = q1 * q2; +// float q13 = q1 * q3; +// float q23 = q2 * q3; +// Tnb_local.x.x = q00 + q11 - q22 - q33; +// Tnb_local.y.y = q00 - q11 + q22 - q33; +// Tnb_local.z.z = q00 - q11 - q22 + q33; +// Tnb_local.y.x = 2*(q12 - q03); +// Tnb_local.z.x = 2*(q13 + q02); +// Tnb_local.x.y = 2*(q12 + q03); +// Tnb_local.z.y = 2*(q23 - q01); +// Tnb_local.x.z = 2*(q13 - q02); +// Tnb_local.y.z = 2*(q23 + q01); + +// // calculate transformation from NED to sensor axes +// Tns = Tbs*Tnb_local; + +// // calculate range from ground plain to centre of sensor fov assuming flat earth +// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + +// // calculate relative velocity in sensor frame +// relVelSensor = Tns*velNED_local; + +// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes +// losPred[0] = relVelSensor.y/range; +// losPred[1] = -relVelSensor.x/range; + +// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); +// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); +// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + +// // 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 +// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); +// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); +// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); +// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); +// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); +// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); +// SKK_LOS[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]); +// SKK_LOS[7] = 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]); +// SKK_LOS[8] = 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]); +// SKK_LOS[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]); +// SKK_LOS[10] = 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]); +// SKK_LOS[11] = 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]); +// SKK_LOS[12] = 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]); +// SKK_LOS[13] = 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]); +// SKK_LOS[14] = SH_LOS[0]; + +// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); +// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// varInnovOptFlow[0] = 1.0f/SK_LOS[0]; +// innovOptFlow[0] = losPred[0] - losData[0]; + +// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag +// obsIndex = 1; +// fuseOptFlowData = 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 +// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(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[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(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[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); +// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// varInnovOptFlow[1] = 1.0f/SK_LOS[1]; +// innovOptFlow[1] = losPred[1] - losData[1]; + +// // reset the observation index +// obsIndex = 0; +// fuseOptFlowData = false; +// } + +// // Check the innovation for consistency and don't fuse if > 3Sigma +// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) +// { +// // correct the state vector +// for (uint8_t j = 0; j < n_states; j++) +// { +// states[j] = states[j] - Kfusion[j] * innovOptFlow[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-12f) +// { +// for (uint8_t j= 0; j<=3; j++) +// { +// float quatMagInv = 1.0f/quatMag; +// states[j] = states[j] * quatMagInv; +// } +// } +// // correct the covariance P = (I - K*H)*P +// // take advantage of the empty columns in 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]; +// } +// } +// ForceSymmetry(); +// ConstrainVariances(); +// } +} + +/* +Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF +This fiter requires optical flow rates that are not motion compensated +Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser +*/ +void AttPosEKF::GroundEKF() +{ + // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption + // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning + if (!inhibitGndState) { + float distanceTravelledSq; + distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); + prevPosN = statesAtRngTime[7]; + prevPosE = statesAtRngTime[8]; + distanceTravelledSq = min(distanceTravelledSq, 100.0f); + Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } - Vector3f velNED_local; - Vector3f relVelSensor; + // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems + Popt[0][0] = 0.0f; + Popt[0][1] = 0.0f; + Popt[1][0] = 0.0f; -// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. - if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) - { - // Sequential fusion of XY components to spread processing load across - // two prediction time steps. + // Fuse range finder data + // Need to check that our range finder tilt angle is less than 30 degrees + float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch); + if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors - // Calculate observation jacobians and Kalman gains - if (fuseOptFlowData) - { - // Copy required states to local variable names - q0 = statesAtOptFlowTime[0]; - q1 = statesAtOptFlowTime[1]; - q2 = statesAtOptFlowTime[2]; - q3 = statesAtOptFlowTime[3]; - vn = statesAtOptFlowTime[4]; - ve = statesAtOptFlowTime[5]; - vd = statesAtOptFlowTime[6]; - pd = statesAtOptFlowTime[9]; - ptd = statesAtOptFlowTime[22]; - velNED_local.x = vn; - velNED_local.y = ve; - velNED_local.z = vd; + // Copy required states to local variable names + q0 = statesAtRngTime[0]; + q1 = statesAtRngTime[1]; + q2 = statesAtRngTime[2]; + q3 = statesAtRngTime[3]; - // calculate rotation from NED to body axes - float q00 = sq(q0); - float q11 = sq(q1); - float q22 = sq(q2); - float q33 = sq(q3); - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - Tnb_local.x.x = q00 + q11 - q22 - q33; - Tnb_local.y.y = q00 - q11 + q22 - q33; - Tnb_local.z.z = q00 - q11 - q22 + q33; - Tnb_local.y.x = 2*(q12 - q03); - Tnb_local.z.x = 2*(q13 + q02); - Tnb_local.x.y = 2*(q12 + q03); - Tnb_local.z.y = 2*(q23 - q01); - Tnb_local.x.z = 2*(q13 - q02); - Tnb_local.y.z = 2*(q23 + q01); - - // calculate transformation from NED to sensor axes - Tns = Tbs*Tnb_local; - - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); - - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED_local; - - // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); - //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); - //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); - - // 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 - SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SKK_LOS[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]); - SKK_LOS[7] = 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]); - SKK_LOS[8] = 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]); - SKK_LOS[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]); - SKK_LOS[10] = 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]); - SKK_LOS[11] = 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]); - SKK_LOS[12] = 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]); - SKK_LOS[13] = 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]); - SKK_LOS[14] = SH_LOS[0]; - - SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - varInnovOptFlow[0] = 1.0f/SK_LOS[0]; - innovOptFlow[0] = losPred[0] - losData[0]; - - // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag - obsIndex = 1; - fuseOptFlowData = false; + // calculate Kalman gains + float SK_RNG[3]; + SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0])); + SK_RNG[2] = 1/SK_RNG[0]; + float K_RNG[2]; + if (!inhibitScaleState) { + K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[0] = 0.0f; } - 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 - SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(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[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(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[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); - Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - varInnovOptFlow[1] = 1.0f/SK_LOS[1]; - innovOptFlow[1] = losPred[1] - losData[1]; - - // reset the observation index - obsIndex = 0; - fuseOptFlowData = false; + if (!inhibitGndState) { + K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[1] = 0.0f; } - // Check the innovation for consistency and don't fuse if > 3Sigma - if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[1]; + + // constrain terrain height to be below the vehicle + flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + + // estimate range to centre of image + range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; + + // Calculate the measurement innovation + innovRng = range - rngMea; + + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); + + // Check the innovation for consistency and don't fuse if out of bounds + if (auxRngTestRatio < 1.0f) { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovOptFlow[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-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in 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]; - } + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_RNG[i] * innovRng; } + // constrain the states + + // constrain focal length to 0.1 to 10 mm + flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); + // constrain altitude + flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + + // correct the covariance matrix + float nextPopt[2][2]; + nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = maxf(nextPopt[0][0],0.0f); + Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = Popt[0][1]; } - 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]; - } - } - ForceSymmetry(); - ConstrainVariances(); } } @@ -2199,6 +2304,24 @@ float AttPosEKF::sq(float valIn) return valIn*valIn; } +float AttPosEKF::maxf(float valIn1, float valIn2) +{ + if (valIn1 >= valIn2) { + return valIn1; + } else { + return valIn2; + } +} + +float AttPosEKF::min(float valIn1, float valIn2) +{ + if (valIn1 <= valIn2) { + return valIn1; + } else { + return valIn2; + } +} + // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { @@ -2395,9 +2518,9 @@ void AttPosEKF::OnGroundCheck() } // don't update terrain offset state if on ground if (onGround) { - inhibitGndHgtState = true; + inhibitGndState = true; } else { - inhibitGndHgtState = false; + inhibitGndState = false; } } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index f8bb7a9c42..8ba1c1573d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -80,6 +80,14 @@ public: airspeedMeasurementSigma = 1.4f; gyroProcessNoise = 1.4544411e-2f; accelProcessNoise = 0.5f; + + gndHgtSigma = 0.1f; // terrain gradient 1-sigma + R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 + flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check + auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter + rngInnovGate = 5.0f; // number of standard deviations applied to the rnage finder innovation consistency check + minFlowRng = 0.01f; //minimum range between ground and flow sensor + moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate } struct mag_state_struct { @@ -125,7 +133,7 @@ public: 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 statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time + float statesAtFlowTime[n_states]; // States at the effective optical flow 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) @@ -204,7 +212,8 @@ public: bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant - bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant + bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant + bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused @@ -223,6 +232,27 @@ public: unsigned storeIndex; + // Optical Flow error estimation + float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators + + // Two state EKF used to estimate focal length scale factor and terrain position + float Popt[2][2]; // state covariance matrix + float flowStates[2]; // flow states [scale factor, terrain position] + float prevPosN; // north position at last measurement + float prevPosE; // east position at last measurement + float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator + float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator + float fScaleFactorVar; // optical flow sensor focal length scale factor variance + Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement + float R_LOS; // Optical flow observation noise variance (rad/sec)^2 + float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold + float auxRngTestRatio; // ratio of range observation innovations to fault threshold + float flowInnovGate; // number of standard deviations used for the innovation consistency check + float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check + float rngInnovGate; // number of standard deviations used for the innovation consistency check + float minFlowRng; // minimum range over which to fuse optical flow measurements + float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate + void updateDtGpsFilt(float dt); void updateDtHgtFilt(float dt); @@ -241,6 +271,8 @@ void FuseRangeFinder(); void FuseOptFlow(); +void GroundEKF(); + 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); @@ -283,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static float sq(float valIn); +static float maxf(float valIn1, float valIn2); + +static float min(float valIn1, float valIn2); + void OnGroundCheck(); void CovarianceInit(); From 752b89b99811e27082be8147c7ff8426f0199478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:24:54 +0200 Subject: [PATCH 27/64] parse hil_optical_flow message publish to flow and range finder topic --- src/modules/mavlink/mavlink_receiver.cpp | 54 +++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 + 2 files changed, 55 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd7898..45ea0e168e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), @@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -363,6 +369,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.ground_distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { @@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vision_position.vx = 0.0f; vision_position.vy = 0.0f; vision_position.vz = 0.0f; - + math::Quaternion q; q.from_euler(pos.roll, pos.pitch, pos.yaw); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 91125736c6..c4e12d8d80 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); @@ -142,6 +143,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; + orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; From 49f1637d7f6486295c5fc7f541fe06ed934688cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:32:24 +0200 Subject: [PATCH 28/64] comment and whitespace --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 45ea0e168e..1115304d42 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -396,6 +396,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + /* Use distance value for range finder report */ struct range_finder_report r; memset(&r, 0, sizeof(f)); @@ -409,7 +410,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_range_pub < 0) { _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); - } else { orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); } From 7db57310d5cdd7295da6c919dafa92d3831296db Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:16 -0700 Subject: [PATCH 29/64] New mavlink_ftp unit test data --- .../mavlink_tests/empty_dir/.gitignore | 0 .../unit_test_data/mavlink_tests/test_234.data | Bin 0 -> 234 bytes .../unit_test_data/mavlink_tests/test_235.data | Bin 0 -> 235 bytes .../unit_test_data/mavlink_tests/test_236.data | Bin 0 -> 236 bytes 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data new file mode 100644 index 0000000000000000000000000000000000000000..3e075aa4f5df1d00f40e8d3b25549fdbe89e5212 GIT binary patch literal 234 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T k$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>6l+?egFUf literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data new file mode 100644 index 0000000000000000000000000000000000000000..61372f7699dab331a855c15bdbfec31e06982c9b GIT binary patch literal 235 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T l$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>FNX4YiIxf literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data new file mode 100644 index 0000000000000000000000000000000000000000..eb7fcb11a5216bd7c3c7e553279d52a945eeb8d1 GIT binary patch literal 236 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T m$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>FVonDQr^! literal 0 HcmV?d00001 From 81275e624e87d5cbb10535ffa2fcc2adbccc793f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:33 -0700 Subject: [PATCH 30/64] Added mavlink_ftp unit test --- makefiles/config_px4fmu-v2_test.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 932f922617..a41670a77b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -54,6 +54,9 @@ MODULES += lib/conversion # LIBRARIES += lib/mathlib/CMSIS +MODULES += modules/unit_test +MODULES += modules/mavlink/mavlink_tests + # # Transitional support - add commands from the NuttX export archive. # From 6f0160bb5db552c921773f278e93d33862811a2a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:50 -0700 Subject: [PATCH 31/64] New mavlink_ftp unit test --- .../mavlink_tests/mavlink_ftp_test.cpp | 788 ++++++++++++++++++ .../mavlink/mavlink_tests/mavlink_ftp_test.h | 109 +++ .../mavlink/mavlink_tests/mavlink_tests.cpp | 52 ++ src/modules/mavlink/mavlink_tests/module.mk | 48 ++ 4 files changed, 997 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_tests.cpp create mode 100644 src/modules/mavlink/mavlink_tests/module.mk diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 0000000000..ebfce6d749 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,788 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/// @file mavlink_ftp_test.cpp +/// @author Don Gagne + +#include +#include +#include +#include + +#include "mavlink_ftp_test.h" +#include "../mavlink_ftp.h" + +/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py +const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { + { "/etc/unit_test_data/mavlink_tests/test_234.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_235.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_236.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets +}; + +const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; +const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file"; + +MavlinkFtpTest::MavlinkFtpTest() : + _ftp_server{}, + _reply_msg{}, + _lastOutgoingSeqNumber{} +{ +} + +MavlinkFtpTest::~MavlinkFtpTest() +{ + +} + +/// @brief Called before every test to initialize the FTP Server. +void MavlinkFtpTest::init(void) +{ + _ftp_server = new MavlinkFTP;; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); + + _cleanup_microsd(); +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::cleanup(void) +{ + delete _ftp_server; + + _cleanup_microsd(); +} + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + return true; +} + +/// @brief Tests for correct response to a message sent with an invalid CRC. +bool MavlinkFtpTest::_bad_crc_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->crc32++; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); + + return true; +} + +/// @brief Tests for correct response to an invalid opcpde. +bool MavlinkFtpTest::_bad_opcode_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = 0xFF; // bogus opcode + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // Set the data size to be one larger than is legal + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response2[] = "Ddev|Detc|Dfs|Dobj"; + + struct _testCase { + const char *dir; + char *response; + bool success; + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, false }, + { "/etc/unit_test_data/mavlink_tests", response1, true }, + { "/", response2, true }, + }; + + for (size_t i=0; iresponse) + 1; + + char *ptr = strtok (test->response, "|"); + while (ptr != nullptr) + { + ptr = strtok (nullptr, "|"); + } + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, expected_data_size); + ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/"; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 4; // offset past top level dirs + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Terminate command on an invalid session. +bool MavlinkFtpTest::_terminate_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) { + ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size); + ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0); + } + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an invalid session. +bool MavlinkFtpTest::_read_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false }, + { "/etc/unit_test_data/mavlink_tests/empty_dir", false, false }, + { _unittest_microsd_dir, false, false }, + { _unittest_microsd_file, false, false }, + { _unittest_microsd_dir, true, true }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ideleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/etc/bogus", false }, + { _unittest_microsd_dir, true }, + { _unittest_microsd_dir, false }, + { "/fs/microsd/bogus/bogus", false }, + }; + + for (size_t i=0; idir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false }, + { _rgReadTestCases[0].file, false }, + { _unittest_microsd_dir, false }, + { _unittest_microsd_file, true }, + { _unittest_microsd_file, false }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test) +{ + ftp_test->_receive_message(msg); +} + +/// @brief Non-Static version of receive_message +void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); +} + +/// @brief Decode and validate the incoming message +bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode + mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message + MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +{ + mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + + // Make sure the targets are correct + ut_compare("Target network non-zero", ftp_msg->target_network, 0); + ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); + ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); + + *payload = reinterpret_cast(ftp_msg->payload); + + // Make sure we have a good CRC + ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); + + // Make sure we have a good sequence number + ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); + + // Bump sequence number for next outgoing message + _lastOutgoingSeqNumber++; + + return true; +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. +uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; +} + +/// @brief Initializes an FTP message into a mavlink message +void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message +{ + uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; + MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); + + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); + + payload->seqNumber = _lastOutgoingSeqNumber; + payload->size = size; + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->crc32 = _payload_crc32(payload); + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server + MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response +{ + mavlink_message_t msg; + + _setup_ftp_msg(payload_header, size, data, &msg); + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd(void) +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +void MavlinkFtpTest::runTests(void) +{ + ut_run_test(_ack_test); + ut_run_test(_bad_crc_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_test); + ut_run_test(_list_test); + ut_run_test(_list_eof_test); + ut_run_test(_open_badfile_test); + ut_run_test(_open_terminate_test); + ut_run_test(_terminate_badsession_test); + ut_run_test(_read_test); + ut_run_test(_read_badsession_test); + ut_run_test(_removedirectory_test); + ut_run_test(_createdirectory_test); + ut_run_test(_removefile_test); +} + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h new file mode 100644 index 0000000000..bbb095a08a --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/// @file mavlink_ftp_test.h +/// @author Don Gagne + +#pragma once + +#include +#include "../mavlink_bridge_header.h" +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest(); + + virtual void init(void); + virtual void cleanup(void); + + virtual void runTests(void); + + static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest&); + MavlinkFtpTest& operator=(const MavlinkFtpTest&); + +private: + bool _ack_test(void); + bool _bad_crc_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message(const mavlink_message_t *msg); + uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); + void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); + bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, + uint8_t size, + const uint8_t *data, + mavlink_file_transfer_protocol_t *ftp_msg_reply, + MavlinkFTP::PayloadHeader **payload_reply); + void _cleanup_microsd(void); + + MavlinkFTP *_ftp_server; + + mavlink_message_t _reply_msg; + + uint16_t _lastOutgoingSeqNumber; + + struct ReadTestCase { + const char *file; + const uint16_t length; + }; + static const ReadTestCase _rgReadTestCases[]; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp new file mode 100644 index 0000000000..d9c1e413a5 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_ftp_tests.cpp + */ + +#include + +#include "mavlink_ftp_test.h" + +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); + +int mavlink_tests_main(int argc, char *argv[]) +{ + MavlinkFtpTest* test = new MavlinkFtpTest; + + test->runTests(); + test->printResults(); + + return 0; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk new file mode 100644 index 0000000000..07cfce1dc9 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = mavlink_tests +SRCS = mavlink_tests.cpp \ + mavlink_ftp_test.cpp \ + ../mavlink_ftp.cpp \ + ../mavlink.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 5000 + +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST From 1969c9ccf6797c4d86f32b9f57332a9e59071251 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:33:43 -0700 Subject: [PATCH 32/64] Improved unit test framework - Added init/cleanup calls before after test - Added ut_compare macro with better failure reporting --- src/modules/unit_test/unit_test.cpp | 5 +++++ src/modules/unit_test/unit_test.h | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 02d1af4818..be3b9461a8 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file, { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } + +void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +{ + warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 32eb8c308c..1a54897094 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); virtual ~UnitTest(); + + virtual void init(void) { }; + virtual void cleanup(void) { }; virtual void runTests(void) = 0; void printResults(void); void printAssert(const char* msg, const char* test, const char* file, int line); + void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); #define ut_assert(message, test) \ do { \ @@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test) } \ } while (0) +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + #define ut_run_test(test) \ do { \ warnx("RUNNING TEST: %s", #test); \ mu_tests_run()++; \ + init(); \ if (!test()) { \ warnx("TEST FAILED: %s", #test); \ mu_tests_failed()++; \ @@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test) warnx("TEST PASSED: %s", #test); \ mu_tests_passed()++; \ } \ + cleanup(); \ } while (0) }; From e7165d6cbf364cb95e45dad998daa7c494de409e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:02 -0700 Subject: [PATCH 33/64] Generator for mavlink_ftp test files --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py new file mode 100644 index 0000000000..a7e68f2a32 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py @@ -0,0 +1,7 @@ +import sys +print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2] +file = open(sys.argv[1], 'w') +for i in range(int(sys.argv[2])): + b = bytearray([i & 0xFF]) + file.write(b) +file.close() \ No newline at end of file From 37ea85968da111f7dc63cdfa8ca02ad6e601a8c3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:20 -0700 Subject: [PATCH 34/64] Tweak for mavlink_ftp api change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd7898..823de5172f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -173,7 +173,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: From 828fb5efd10355d0d33b05fcf597bc4c05cd4db4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:36:32 -0700 Subject: [PATCH 35/64] Additional FTP command support plus unit test - Restructured to a flatter class implementation - Added support for create/remove directory, remove file - Refactored error codes and command opcodes - Added unit test support - Fixed bugs found by unit test --- src/modules/mavlink/mavlink_ftp.cpp | 445 +++++++++++++++++++--------- src/modules/mavlink/mavlink_ftp.h | 285 +++++++----------- 2 files changed, 423 insertions(+), 307 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 5b65dc369d..72ede87976 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -31,38 +31,39 @@ * ****************************************************************************/ +/// @file mavlink_ftp.cpp +/// @author px4dev, Don Gagne + #include #include #include #include #include +#include #include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" // Uncomment the line below to get better debug output. Never commit with this left on. //#define MAVLINK_FTP_DEBUG -MavlinkFTP *MavlinkFTP::_server; - MavlinkFTP * -MavlinkFTP::getServer() +MavlinkFTP::get_server(void) { - // XXX this really cries out for some locking... - if (_server == nullptr) { - _server = new MavlinkFTP; - } - return _server; + static MavlinkFTP server; + return &server; } MavlinkFTP::MavlinkFTP() : - _session_fds{}, - _workBufs{}, - _workFree{}, - _lock{} + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} { // initialise the request freelist - dq_init(&_workFree); - sem_init(&_lock, 0, 1); + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); // initialize session list for (size_t i=0; idecode(mavlink, msg)) { - - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); - } else { - _qFree(req); + if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_msg_file_transfer_protocol_decode(msg, &req->message); + +#ifdef MAVLINK_FTP_UNIT_TEST + if (!_utRcvMsgFunc) { + warnx("Incorrectly written unit test\n"); + return; + } + // We use fake ids when unit testing + req->serverSystemId = MavlinkFtpTest::serverSystemId; + req->serverComponentId = MavlinkFtpTest::serverComponentId; + req->serverChannel = MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + req->serverSystemId = mavlink->get_system_id(); + req->serverComponentId = mavlink->get_component_id(); + req->serverChannel = mavlink->get_channel(); +#endif + + // This is the system id we want to target when sending + req->targetSystemId = msg->sysid; + + if (req->message.target_system == req->serverSystemId) { + req->mavlink = mavlink; +#ifdef MAVLINK_FTP_UNIT_TEST + // We are running in Unit Test mode. Don't queue, just call _worket directly. + _process_request(req); +#else + // We are running in normal mode. Queue the request to the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); +#endif + return; } } + + _return_request(req); } +/// @brief Queued static work queue routine to handle mavlink messages void -MavlinkFTP::_workerTrampoline(void *arg) +MavlinkFTP::_worker_trampoline(void *arg) { - auto req = reinterpret_cast(arg); - auto server = MavlinkFTP::getServer(); + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); // call the server worker with the work item - server->_worker(req); + server->_process_request(req); } +/// @brief Processes an FTP message void -MavlinkFTP::_worker(Request *req) +MavlinkFTP::_process_request(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + ErrorCode errorCode = kErrNone; - uint32_t messageCRC; // basic sanity checks; must validate length before use - if (hdr->size > kMaxDataLength) { - errorCode = kErrNoRequest; + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; goto out; } // check request CRC to make sure this is one of ours - messageCRC = hdr->crc32; - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - if (crc32(req->rawData(), req->dataSize()) != messageCRC) { - errorCode = kErrNoRequest; + if (_payload_crc32(payload) != payload->crc32) { + errorCode = kErrCrc; goto out; warnx("ftp: bad crc"); } #ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif - switch (hdr->opcode) { + switch (payload->opcode) { case kCmdNone: break; - case kCmdTerminate: - errorCode = _workTerminate(req); + case kCmdTerminateSession: + errorCode = _workTerminate(payload); break; - case kCmdReset: - errorCode = _workReset(); + case kCmdResetSessions: + errorCode = _workReset(payload); break; - case kCmdList: - errorCode = _workList(req); + case kCmdListDirectory: + errorCode = _workList(payload); break; - case kCmdOpen: - errorCode = _workOpen(req, false); + case kCmdOpenFile: + errorCode = _workOpen(payload, false); break; - case kCmdCreate: - errorCode = _workOpen(req, true); + case kCmdCreateFile: + errorCode = _workOpen(payload, true); break; - case kCmdRead: - errorCode = _workRead(req); + case kCmdReadFile: + errorCode = _workRead(payload); break; - case kCmdWrite: - errorCode = _workWrite(req); + case kCmdWriteFile: + errorCode = _workWrite(payload); break; - case kCmdRemove: - errorCode = _workRemove(req); + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); break; + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + default: - errorCode = kErrNoRequest; + errorCode = kErrUnknownCommand; break; } out: // handle success vs. error if (errorCode == kErrNone) { - hdr->opcode = kRspAck; + payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); - hdr->opcode = kRspNak; - hdr->size = 1; - hdr->data[0] = errorCode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = errno; + } } + // respond to the request _reply(req); - // free the request buffer back to the freelist - _qFree(req); + _return_request(req); } +/// @brief Sends the specified FTP reponse message out through mavlink void MavlinkFTP::_reply(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - hdr->seqNumber = req->header()->seqNumber + 1; + payload->seqNumber = payload->seqNumber + 1; - // message is assumed to be already constructed in the request buffer, so generate the CRC - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - hdr->crc32 = crc32(req->rawData(), req->dataSize()); + payload->crc32 = _payload_crc32(payload); - // then pack and send the reply back to the request source - req->reply(); + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#endif + mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id + req->serverComponentId, // Sender component id + req->serverChannel, // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + req->targetSystemId, // Target system id + 0, // Target component id + (const uint8_t*)payload); // Payload to pack into message + + bool success = true; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#endif + + if (!success) { + warnx("FTP TX ERR"); + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", + req->serverSystemId, + req->serverComponentId, + req->serverChannel, + msg.checksum); + } +#endif } +/// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(Request *req) +MavlinkFTP::_workList(PayloadHeader* payload) { - auto hdr = req->header(); - char dirPath[kMaxDataLength]; - strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); DIR *dp = opendir(dirPath); if (dp == nullptr) { warnx("FTP: can't open path '%s'", dirPath); - return kErrNotDir; + return kErrFailErrno; } #ifdef MAVLINK_FTP_DEBUG - warnx("FTP: list %s offset %d", dirPath, hdr->offset); + warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif ErrorCode errorCode = kErrNone; @@ -239,19 +320,19 @@ MavlinkFTP::_workList(Request *req) unsigned offset = 0; // move to the requested offset - seekdir(dp, hdr->offset); + seekdir(dp, payload->offset); for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrIO; + errorCode = kErrFailErrno; break; } // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { + if (payload->offset != 0 && offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked // to seek past EOF. errorCode = kErrEOF; @@ -299,94 +380,95 @@ MavlinkFTP::_workList(Request *req) } // Move the data into the buffer - hdr->data[offset++] = direntType; - strcpy((char *)&hdr->data[offset], buf); + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); #ifdef MAVLINK_FTP_DEBUG - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); #endif offset += nameLen + 1; } closedir(dp); - hdr->size = offset; + payload->size = offset; return errorCode; } +/// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(Request *req, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) { - auto hdr = req->header(); - - int session_index = _findUnusedSession(); + int session_index = _find_unused_session(); if (session_index < 0) { - return kErrNoSession; + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; } - + + char *filename = _data_as_cstring(payload); uint32_t fileSize = 0; if (!create) { struct stat st; - if (stat(req->dataAsCString(), &st) != 0) { - return kErrNotFile; + if (stat(filename, &st) != 0) { + return kErrFailErrno; } fileSize = st.st_size; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(filename, oflag); if (fd < 0) { - return create ? kErrPerm : kErrNotFile; + return kErrFailErrno; } _session_fds[session_index] = fd; - hdr->session = session_index; + payload->session = session_index; if (create) { - hdr->size = 0; + payload->size = 0; } else { - hdr->size = sizeof(uint32_t); - *((uint32_t*)hdr->data) = fileSize; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; } return kErrNone; } +/// @brief Responds to a Read command MavlinkFTP::ErrorCode -MavlinkFTP::_workRead(Request *req) +MavlinkFTP::_workRead(PayloadHeader* payload) { - auto hdr = req->header(); + int session_index = payload->session; - int session_index = hdr->session; - - if (!_validSession(session_index)) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } // Seek to the specified position #ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", hdr->offset); + warnx("seek %d", payload->offset); #endif - if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); return kErrEOF; } - int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof warnx("read fail %d", bytes_read); - return kErrIO; + return kErrFailErrno; } - hdr->size = bytes_read; + payload->size = bytes_read; return kErrNone; } +/// @brief Responds to a Write command MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(Request *req) +MavlinkFTP::_workWrite(PayloadHeader* payload) { #if 0 // NYI: Coming soon @@ -409,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; #else - return kErrPerm; + return kErrUnknownCommand; #endif } +/// @brief Responds to a RemoveFile command MavlinkFTP::ErrorCode -MavlinkFTP::_workRemove(Request *req) +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) { - //auto hdr = req->header(); - - // for now, send error reply - return kErrPerm; + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } } +/// @brief Responds to a Terminate command MavlinkFTP::ErrorCode -MavlinkFTP::_workTerminate(Request *req) +MavlinkFTP::_workTerminate(PayloadHeader* payload) { - auto hdr = req->header(); - - if (!_validSession(hdr->session)) { - return kErrNoSession; + if (!_valid_session(payload->session)) { + return kErrInvalidSession; } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; return kErrNone; } +/// @brief Responds to a Reset command MavlinkFTP::ErrorCode -MavlinkFTP::_workReset(void) +MavlinkFTP::_workReset(PayloadHeader* payload) { for (size_t i=0; isize = 0; + return kErrNone; } +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Returns true if the specified session is a valid open session bool -MavlinkFTP::_validSession(unsigned index) +MavlinkFTP::_valid_session(unsigned index) { if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; @@ -458,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index) return true; } +/// @brief Returns an unused session index int -MavlinkFTP::_findUnusedSession(void) +MavlinkFTP::_find_unused_session(void) { for (size_t i=0; isize < kMaxDataLength) { - requestData()[header()->size] = '\0'; + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; } else { - requestData()[kMaxDataLength - 1] = '\0'; + payload->data[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)&(header()->data[0]); + return (char *)&(payload->data[0]); +} + +/// @brief Returns a unused Request entry. NULL if none available. +MavlinkFTP::Request * +MavlinkFTP::_get_request(void) +{ + _lock_request_queue(); + Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); + _unlock_request_queue(); + return req; +} + +/// @brief Locks a semaphore to provide exclusive access to the request queue +void +MavlinkFTP::_lock_request_queue(void) +{ + do {} + while (sem_wait(&_request_queue_sem) != 0); +} + +/// @brief Unlocks the semaphore providing exclusive access to the request queue +void +MavlinkFTP::_unlock_request_queue(void) +{ + sem_post(&_request_queue_sem); +} + +/// @brief Returns a no longer needed request to the queue +void +MavlinkFTP::_return_request(Request *req) +{ + _lock_request_queue(); + dq_addlast(&req->work.dq, &_request_queue); + _unlock_request_queue(); +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. +uint32_t +MavlinkFTP::_payload_crc32(PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1dd8f102e7..b4cc154d11 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -33,17 +33,8 @@ #pragma once -/** - * @file mavlink_ftp.h - * - * MAVLink remote file server. - * - * A limited number of requests (currently 2) may be outstanding at a time. - * Additional messages will be discarded. - * - * Messages consist of a fixed header, followed by a data area. - * - */ +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne #include #include @@ -54,183 +45,131 @@ #include "mavlink_messages.h" #include "mavlink_main.h" +class MavlinkFtpTest; + +/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded. class MavlinkFTP { public: + /// @brief Returns the one Mavlink FTP server in the system. + static MavlinkFTP* get_server(void); + + /// @brief Contructor is only public so unit test code can new objects. MavlinkFTP(); + + /// @brief Adds the specified message to the work queue. + void handle_message(Mavlink* mavlink, mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + /// @brief Sets up the server to run in unit test mode. + /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. + /// @param ftp_test MavlinkFtpTest object which the function is associated with + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test); - static MavlinkFTP *getServer(); - - // static interface - void handle_message(Mavlink* mavlink, - mavlink_message_t *msg); - -private: - - static const unsigned kRequestQueueSize = 2; - - static MavlinkFTP *_server; - - /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the - /// structure ourselves to 32 bit alignment which should get us what we want. - struct RequestHeader + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to + /// 32 bit alignment to avoid usage of any pack pragmas. + struct PayloadHeader { - uint16_t seqNumber; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t padding[3]; - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data[]; + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; ///< 32 bit aligment padding + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode }; - + + /// @brief Command opcodes enum Opcode : uint8_t { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFile, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Appends bytes to file in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + + kRspAck, ///< Ack response + kRspNak ///< Nak response }; - + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t - { + { kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; - - int _findUnusedSession(void); - bool _validSession(unsigned index); - - static const unsigned kMaxSession = 2; - int _session_fds[kMaxSession]; - - class Request + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrCrc ///< CRC on Payload is incorrect + }; + +private: + /// @brief Unit of work which is queued to work_queue + struct Request { - public: - union { - dq_entry_t entry; - work_s work; - }; - - bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - _systemId = fromMessage->sysid; - _mavlink = mavlink; - mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); - return _message.target_system == _mavlink->get_system_id(); - } - return false; - } - - void reply() { - - // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the - // flat memory architecture, as we're operating between threads here. - mavlink_message_t msg; - msg.checksum = 0; - unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id - _mavlink->get_component_id(), // Sender component id - _mavlink->get_channel(), // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - _systemId, // Target system id - 0, // Target component id - rawData()); // Payload to pack into message - - _mavlink->lockMessageBufferMutex(); - bool success = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } -#endif - } - - uint8_t *rawData() { return &_message.payload[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } - uint8_t *requestData() { return &(header()->data[0]); } - unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - mavlink_channel_t channel() { return _mavlink->get_channel(); } - - char *dataAsCString(); - - private: - Mavlink *_mavlink; - mavlink_file_transfer_protocol_t _message; - uint8_t _systemId; + work_s work; ///< work queue entry + Mavlink *mavlink; ///< Mavlink to reply to + uint8_t serverSystemId; ///< System ID to send from + uint8_t serverComponentId; ///< Component ID to send from + uint8_t serverChannel; ///< Channel to send to + uint8_t targetSystemId; ///< System ID to target reply to + mavlink_file_transfer_protocol_t message; ///< Protocol message }; + + Request *_get_request(void); + void _return_request(Request *req); + void _lock_request_queue(void); + void _unlock_request_queue(void); + + uint32_t _payload_crc32(PayloadHeader *hdr); + + char *_data_as_cstring(PayloadHeader* payload); + + static void _worker_trampoline(void *arg); + void _process_request(Request *req); + void _reply(Request *req); - static const char kDirentFile = 'F'; - static const char kDirentDir = 'D'; - static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader* payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(PayloadHeader *payload); - /// Request worker; runs on the low-priority work queue to service - /// remote requests. - /// - static void _workerTrampoline(void *arg); - void _worker(Request *req); - - /// Reply to a request (XXX should be a Request method) - /// - void _reply(Request *req); - - ErrorCode _workList(Request *req); - ErrorCode _workOpen(Request *req, bool create); - ErrorCode _workRead(Request *req); - ErrorCode _workWrite(Request *req); - ErrorCode _workRemove(Request *req); - ErrorCode _workTerminate(Request *req); - ErrorCode _workReset(); - - // work freelist - Request _workBufs[kRequestQueueSize]; - dq_queue_t _workFree; - sem_t _lock; - - void _qLock() { do {} while (sem_wait(&_lock) != 0); } - void _qUnlock() { sem_post(&_lock); } - - void _qFree(Request *req) { - _qLock(); - dq_addlast(&req->entry, &_workFree); - _qUnlock(); - } - - Request *_dqFree() { - _qLock(); - auto req = reinterpret_cast(dq_remfirst(&_workFree)); - _qUnlock(); - return req; - } - + static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests + Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work + dq_queue_t _request_queue; ///< Queue of available Request buffers + sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue + + int _find_unused_session(void); + bool _valid_session(unsigned index); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command + + /// @brief Maximum data size in RequestHeader::data + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); + + static const unsigned kMaxSession = 2; ///< Max number of active sessions + int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot + + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; }; From 8c2a158e121e0a716a4b2d802687b0314c0217ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 12:15:27 +0200 Subject: [PATCH 36/64] Trust the laser sensor more --- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 8ba1c1573d..a607955a89 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -85,7 +85,7 @@ public: R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 5.0f; // number of standard deviations applied to the rnage finder innovation consistency check + rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check minFlowRng = 0.01f; //minimum range between ground and flow sensor moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate } From 7dfc67d032220c8c679848c684703e13e8d13080 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 17:52:24 +0200 Subject: [PATCH 37/64] fix mission item to mavink waypoint conversion --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d436c95e90..a3c127cdc5 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -798,10 +798,10 @@ int MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; } switch (mission_item->nav_cmd) { From 1d038eed047358eee30513673bbcb29bf93dd78e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Sep 2014 22:44:16 +0400 Subject: [PATCH 38/64] UAVCAN: GNSS fix message update --- src/modules/uavcan/sensors/gnss.cpp | 8 +++----- uavcan | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 0d67aad476..24afe6aaf4 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -43,8 +43,6 @@ #include #include -#define MM_PER_CM 10 // Millimeters per centimeter - const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : @@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 3 Sep 2014 21:18:32 +0200 Subject: [PATCH 39/64] FOH mode for altitude This introduces a parameter for the navigator. When enabled the navigator publishes a first order old (FOH) type altitude setpoint instead of the default zero order hold. For takeoff and landing the FOH mode is not active. The FOH altitude is calculated such that the sp reaches the altitude of the waypoint when the system is at a horizontal distance equal to the acceptance radius. Also the altitude setpoint will only converge towards the waypoint altitude but never diverge. --- src/modules/navigator/mission.cpp | 86 +++++++++++++++++++++++++- src/modules/navigator/mission.h | 25 +++++++- src/modules/navigator/mission_params.c | 13 ++++ 3 files changed, 121 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3eda..7000dd6c0c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -36,12 +36,14 @@ * Helper class to access missions * * @author Julian Oes + * @author Thomas Gubler */ #include #include #include #include +#include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include #include @@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), _param_dist_1wp(this, "DIST_1WP"), + _param_altmode(this, "ALTMODE"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false) + _dist_1wp_ok(false), + _missionFeasiblityChecker(), + _min_current_sp_distance_xy(FLT_MAX), + _mission_item_previous_alt(NAN), + _distance_current_previous(0.0f) { /* load initial params */ updateParams(); @@ -144,6 +152,8 @@ Mission::on_active() advance_mission(); set_mission_items(); + } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { + altitude_sp_foh_update(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { @@ -202,7 +212,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); @@ -313,11 +323,19 @@ Mission::set_mission_items() /* make sure param is up to date */ updateParams(); + /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ + altitude_sp_foh_reset(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ + if (pos_sp_triplet->previous.valid) { + _mission_item_previous_alt = _mission_item.altitude; + } + /* get home distance state */ bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ @@ -446,9 +464,73 @@ Mission::set_mission_items() } } + /* Save the distance between the current sp and the previous one */ + if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { + _distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + pos_sp_triplet->previous.lat, + pos_sp_triplet->previous.lon); + } + _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::altitude_sp_foh_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_mission_item_previous_alt)) { + return; + } + + /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ + if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + + /* Calculate distance to current waypoint */ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + /* Save distance to waypoint if it is the smallest ever achieved */ + _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + + /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt + * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ + if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { + pos_sp_triplet->current.alt = _mission_item.altitude; + } else { + /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp + * of the mission item as it is used to check if the mission item is reached + * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance + * radius around the current waypoint + **/ + float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float a = _mission_item_previous_alt - grad * _distance_current_previous; + pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; + + } + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Mission::altitude_sp_foh_reset() +{ + _min_current_sp_distance_xy = FLT_MAX; +} + bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..f7d12da702 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -36,6 +36,7 @@ * Navigator mode to access missions * * @author Julian Oes + * @author Thomas Gubler */ #ifndef NAVIGATOR_MISSION_H @@ -75,6 +76,11 @@ public: virtual void on_active(); + enum mission_altitude_mode { + MISSION_ALTMODE_ZOH = 0, + MISSION_ALTMODE_FOH =1 + }; + private: /** * Update onboard mission topic @@ -102,6 +108,16 @@ private: */ void set_mission_items(); + /** + * Updates the altitude sp to follow a foh + */ + void altitude_sp_foh_update(); + + /** + * Resets the altitude sp foh logic + */ + void altitude_sp_foh_reset(); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful @@ -136,6 +152,7 @@ private: control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; + control::BlockParamInt _param_altmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -157,7 +174,13 @@ private: bool _inited; bool _dist_1wp_ok; - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + + float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ + float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, + can be replaced by a full copy of the previous mission item if needed*/ + float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, + only use if current and previous are valid */ }; #endif diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d15e1e771b..04c01fe51c 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @group Mission */ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); + +/** + * Altitude setpoint mode + * + * 0: the system will follow a zero order hold altitude setpoint + * 1: the system will follow a first order hold altitude setpoint + * values follow the definition in enum mission_altitude_mode + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ALTMODE, 0); From 9cf5e6f7b268acd732a14e577752dc32aeaf9430 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 22:08:03 +0200 Subject: [PATCH 40/64] whitespace --- src/modules/navigator/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f7d12da702..a91c899687 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,7 +78,7 @@ public: enum mission_altitude_mode { MISSION_ALTMODE_ZOH = 0, - MISSION_ALTMODE_FOH =1 + MISSION_ALTMODE_FOH = 1 }; private: From 67422c9896fb952ccf6db555aeb6f99224f8178b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:41:13 +0200 Subject: [PATCH 41/64] foh alt mode: never sink below previous wp alt --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7000dd6c0c..b33b2049fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -502,8 +502,10 @@ Mission::altitude_sp_foh_update() float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - /* Save distance to waypoint if it is the smallest ever achieved */ - _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + /* Save distance to waypoint if it is the smallest ever achieved, however make sure that + * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */ + _min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy), + _distance_current_previous); /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ From 4c7d6707936718760639ee60b3c27698f0ca119c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:04:42 +0200 Subject: [PATCH 42/64] loiter mission items: better reached check for FW --- src/modules/navigator/mission_block.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce6..157a1cf712 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field + * Therefore the item is marked as reached once the system reaches the loiter + * radius (+ some margin). Time inside and turn count is handled elsewhere. + */ + if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + _waypoint_position_reached = true; + } + } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From 81837dcdde4bdef31fc153cd1f893e2fefc74c58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:18:25 +0200 Subject: [PATCH 43/64] update comment --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 157a1cf712..723caec7ce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -118,14 +118,14 @@ MissionBlock::is_mission_item_reached() _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the - * coordinates with a radius equal to the loiter_radius field + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } - } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From f315be54162c3284742cd2195d9c78c153289e66 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Sep 2014 10:36:51 +0200 Subject: [PATCH 44/64] FW: in seatbelt/althold on ground reset integrators --- .../fw_att_control/fw_att_control_main.cpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) 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 ad873203eb..c60d8d3489 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -124,6 +125,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ @@ -139,6 +141,7 @@ private: struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ @@ -275,6 +278,11 @@ private: */ void global_pos_poll(); + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _params_sub(-1), _manual_sub(-1), _global_pos_sub(-1), + _vehicle_status_sub(-1), /* publications */ _rate_sp_pub(-1), @@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators = {}; _actuators_airframe = {}; _global_pos = {}; + _vehicle_status = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll() } } +void +FixedwingAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main() vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); + vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; @@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main() global_pos_poll(); + vehicle_status_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main() } } + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; From e056fbb7038e4274fc54f4e19e66c8174ffff28f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 10:59:35 +0200 Subject: [PATCH 45/64] add heightrate ff for tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 6 + .../fw_pos_control_l1_main.cpp | 5 + .../fw_pos_control_l1_params.c | 125 +++++++++--------- 4 files changed, 78 insertions(+), 60 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776fb..023bd71bfc 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 36ae4ecaf2..8ac31de6f5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -47,6 +47,7 @@ public: _rollComp(0.0f), _spdWeight(0.5f), _heightrate_p(0.0f), + _heightrate_ff(0.0f), _speedrate_p(0.0f), _throttle_dem(0.0f), _pitch_dem(0.0f), @@ -220,6 +221,10 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } @@ -256,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 84c14be018..d339b1c4d0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -211,6 +211,7 @@ private: float max_climb_rate; float climbout_diff; float heightrate_p; + float heightrate_ff; float speedrate_p; float throttle_damp; float integrator_gain; @@ -256,6 +257,7 @@ private: param_t max_climb_rate; param_t climbout_diff; param_t heightrate_p; + param_t heightrate_ff; param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; @@ -494,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ @@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); @@ -600,6 +604,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_heightrate_ff(_parameters.heightrate_ff); _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 890ab3bad2..847ecbb5cb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * * @group L1 Control @@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide * some aerodynamic drag from a turning prop to improve the descent rate. * * For aircraft with internal combustion engine this parameter should be set @@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Throttle limit value before flare * - * This throttle value will be set as throttle limit at FW_LND_TLALT, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); /** * Maximum climb rate * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * * @group L1 Control @@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); /** * Minimum descent rate * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * * @group Fixed Wing TECS @@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); /** * Integrator gain * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and * increases overshoot. * * @group Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** * Complementary filter "omega" parameter for height * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); /** * Complementary filter "omega" parameter for speed * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** * Pitch damping factor * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned * properly. * * @group Fixed Wing TECS @@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +/** + * Height rate FF factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); + /** * Speed rate P factor * From daf16184202b02119aa1ac83cb82cf85979d43f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 12:42:42 +0200 Subject: [PATCH 46/64] additional upper pitch limit during launch The pitch limit can be used by the laucnhdetector to limit pitch during critical phases of a launch. For example this can be used to limit pitch while attached to a bungee differently from the standard pitch limit. --- .../launchdetection/CatapultLaunchMethod.cpp | 13 +++++++++++- .../launchdetection/CatapultLaunchMethod.h | 4 ++++ src/lib/launchdetection/LaunchDetector.cpp | 20 +++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/LaunchMethod.h | 3 +++ .../launchdetection/launchdetection_params.c | 16 ++++++++++++++- .../fw_pos_control_l1_main.cpp | 12 ++++++++--- 7 files changed, 66 insertions(+), 5 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 65ae461dbc..2ea1c414bd 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -52,7 +52,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : state(LAUNCHDETECTION_RES_NONE), thresholdAccel(this, "A"), thresholdTime(this, "T"), - motorDelay(this, "MDEL") + motorDelay(this, "MDEL"), + pitchMaxPreThrottle(this, "PMAX") { } @@ -118,4 +119,14 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { + /* If motor is turned on do not impose the extra limit on maximum pitch */ + if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + return pitchMaxDefault; + } else { + return pitchMaxPreThrottle.get(); + } + +} + } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index d918c3a763..321dfb1de4 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -59,6 +59,7 @@ public: void update(float accel_x); LaunchDetectionResult getLaunchDetected() const; void reset(); + float getPitchMax(float pitchMaxDefault); private: hrt_abstime last_timestamp; @@ -70,6 +71,9 @@ private: control::BlockParamFloat thresholdAccel; control::BlockParamFloat thresholdTime; control::BlockParamFloat motorDelay; + control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on. + Can be used to make sure that the AC does not climb + too much while attached to a bungee */ }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2958c0a315..52f5c32576 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -105,4 +105,24 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } +float LaunchDetector::getPitchMax(float pitchMaxDefault) { + if (!launchdetection_on.get()) { + return pitchMaxDefault; + } + + /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, + * otherwise use the default limit */ + if (activeLaunchDetectionMethodIndex < 0) { + if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) { + return pitchMaxDefault; + } else { + return launchMethods[0]->getPitchMax(pitchMaxDefault); + } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); + } + +} + + } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index b48e724bad..4215b49d28 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -64,6 +64,9 @@ public: float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ + float getPitchMax(float pitchMaxDefault); + // virtual bool getLaunchDetected(); protected: private: diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index d2f091cea7..8b5220cb3c 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -62,6 +62,9 @@ public: virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; + /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ + virtual float getPitchMax(float pitchMaxDefault) = 0; + protected: private: }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index d35eb11f60..e3aa7ab2d2 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** * Motor delay * - * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * * @unit seconds @@ -88,6 +88,20 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); + +/** + * Maximum pitch before the throttle is powered up (during motor delay phase) + * + * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. + * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + * + * @unit deg + * @min 0 + * @max 45 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); + /** * Throttle setting while detecting launch. * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 84c14be018..402b171f5d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1115,7 +1115,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_rad = math::radians( + launchDetector.getPitchMax(_parameters.pitch_limit_max)); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ @@ -1123,7 +1129,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), + takeoff_pitch_max_rad, _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, @@ -1199,7 +1205,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Copy thrust and pitch values from tecs * making sure again that the correct thrust is used, - * without depending on library calls */ + * without depending on library calls for safety reasons */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); From df181455ebdcfacda73615adba40fa224b4d074c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 13:00:26 +0200 Subject: [PATCH 47/64] launch pitch limit: add mtecs interface --- .../fw_pos_control_l1_main.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 402b171f5d..81e25da995 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -381,7 +381,8 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -1117,8 +1118,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - float takeoff_pitch_max_rad = math::radians( - launchDetector.getPitchMax(_parameters.pitch_limit_max)); + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* apply minimum pitch and limit roll if target altitude is not within climbout_diff * meters */ @@ -1137,7 +1138,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), @@ -1391,7 +1393,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode) + tecs_mode mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1406,6 +1408,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else { limitOverride.disablePitchMinOverride(); } + + if (pitch_max_special) { + /* Use the maximum pitch from the argument */ + limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { + /* use pitch max set by MT param */ + limitOverride.disablePitchMaxOverride(); + } _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { From f4664daa8ed80e369c8600f2f1d1ee8d5be328af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:08:56 +0200 Subject: [PATCH 48/64] Add license file --- LICENSE.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 LICENSE.md diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 0000000000..2ad83eba48 --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,41 @@ +The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required +to be made under the same license. Any exception to this general rule is listed below. + + /**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + - PX4 middleware: BSD 3-clause + - PX4 flight control stack: BSD 3-clause + - NuttX operating system: BSD 3-clause + - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. \ No newline at end of file From 35a6074419b3dcf567f23db74b8ea53eff20c9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:12:38 +0200 Subject: [PATCH 49/64] Added readme --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000000..84fb02e3b9 --- /dev/null +++ b/README.md @@ -0,0 +1,10 @@ +## PX4 Aerial Middleware and Flight Control Stack ## + +* Official Website: http://px4.io +* License: BSD 3-clause (see LICENSE.md) +* Supported airframes: + * Multicopters + * Fixed wing +* Binaries (always up-to-date from master): + * [Downloads](https://pixhawk.org/downloads) +* Mailing list: [Google Groups](http://groups.google.com/group/px4users) From 564c9b7b60aaa1187570e188fe7d1e19945d40b5 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:02:25 +0400 Subject: [PATCH 50/64] FTP: Add req_opcode field for return request opcode in response message. --- src/modules/mavlink/mavlink_ftp.cpp | 3 ++- src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 72ede87976..ae9246ece0 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -224,12 +224,14 @@ MavlinkFTP::_process_request(Request *req) out: // handle success vs. error if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); + payload->req_opcode = payload->opcode; payload->opcode = kRspNak; payload->size = 1; payload->data[0] = errorCode; @@ -654,7 +656,6 @@ MavlinkFTP::_payload_crc32(PayloadHeader *payload) payload->crc32 = 0; payload->padding[0] = 0; payload->padding[1] = 0; - payload->padding[2] = 0; uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); payload->crc32 = saveCRC; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b4cc154d11..1fb50a17e6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -76,7 +76,8 @@ public: uint8_t session; ///< Session id for read and write commands uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data - uint8_t padding[3]; ///< 32 bit aligment padding + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode From e7ae13a58e83263973feab3630f90f077786fcc3 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:17:11 +0400 Subject: [PATCH 51/64] FTP: Make responses start from opcode 128. --- src/modules/mavlink/mavlink_ftp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1fb50a17e6..85e175d447 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,7 +98,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kRspAck, ///< Ack response + kRspAck = 128, ///< Ack response kRspNak ///< Nak response }; From 0d2e250d119a5a14c8757982c96b2afef09c4d0d Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:36:41 +0400 Subject: [PATCH 52/64] FTP: Remove CRC32 from protocol. Extra crc not needed because mavlink already has crc16. --- src/modules/mavlink/mavlink_ftp.cpp | 27 +++------------------------ src/modules/mavlink/mavlink_ftp.h | 8 ++------ 2 files changed, 5 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ae9246ece0..b847fc6258 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,7 +34,6 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne -#include #include #include #include @@ -161,13 +160,6 @@ MavlinkFTP::_process_request(Request *req) goto out; } - // check request CRC to make sure this is one of ours - if (_payload_crc32(payload) != payload->crc32) { - errorCode = kErrCrc; - goto out; - warnx("ftp: bad crc"); - } - #ifdef MAVLINK_FTP_DEBUG printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif @@ -255,8 +247,9 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - - payload->crc32 = _payload_crc32(payload); + payload->reserved[0] = 0; + payload->reserved[1] = 0; + payload->reserved[2] = 0; mavlink_message_t msg; msg.checksum = 0; @@ -647,17 +640,3 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. -uint32_t -MavlinkFTP::_payload_crc32(PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 85e175d447..ae7955ab73 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,8 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t padding[2]; ///< 32 bit aligment padding - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint16_t reserved[3]; ///< reserved area uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; @@ -112,8 +111,7 @@ public: kErrInvalidSession, ///< Session is not currently open kErrNoSessionsAvailable, ///< All available Sessions in use kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrCrc ///< CRC on Payload is incorrect + kErrUnknownCommand ///< Unknown command opcode }; private: @@ -135,8 +133,6 @@ private: void _lock_request_queue(void); void _unlock_request_queue(void); - uint32_t _payload_crc32(PayloadHeader *hdr); - char *_data_as_cstring(PayloadHeader* payload); static void _worker_trampoline(void *arg); From 745707d19377e2c650258ea77570a5f84a71c1b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:22:03 +0400 Subject: [PATCH 53/64] FTP: remove reserved uint32 from header. --- src/modules/mavlink/mavlink_ftp.cpp | 5 ++--- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b847fc6258..a5c96274be 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,9 +247,8 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->reserved[0] = 0; - payload->reserved[1] = 0; - payload->reserved[2] = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index ae7955ab73..4892e548c0 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,7 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint16_t reserved[3]; ///< reserved area + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; From ed66097ebc4808587301bd569b5bef6c312800c4 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:54:42 +0400 Subject: [PATCH 54/64] FTP: Update unit test for new header size. _list_test failed. --- .../{test_234.data => test_238.data} | Bin 234 -> 238 bytes .../{test_235.data => test_239.data} | Bin 235 -> 239 bytes .../{test_236.data => test_240.data} | Bin 236 -> 240 bytes .../mavlink_tests/mavlink_ftp_test.cpp | 57 ++---------------- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 2 - 5 files changed, 6 insertions(+), 53 deletions(-) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_234.data => test_238.data} (85%) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_235.data => test_239.data} (84%) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_236.data => test_240.data} (84%) diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data similarity index 85% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data index 3e075aa4f5df1d00f40e8d3b25549fdbe89e5212..973de16e5965e6caeccd2b13cd41421766c1bf04 100644 GIT binary patch delta 11 TcmaFG_>OVHE0$NU-@F9?B{v5L delta 6 NcmaFI_=<7DD*y{r162S3 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data similarity index 84% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data index 61372f7699dab331a855c15bdbfec31e06982c9b..7e8764c031918329d226d5e77f4649ce60c71a25 100644 GIT binary patch delta 11 TcmaFO_?~gXYnIn<-o66>C0hp$ delta 6 NcmaFQ_?mITYXA#h16Tk6 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data similarity index 84% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data index eb7fcb11a5216bd7c3c7e553279d52a945eeb8d1..f3fe31687fc2daf01c80768dc521bd8f659f8db2 100644 GIT binary patch delta 11 TcmaFE_payload)->crc32++; - - _ftp_server->handle_message(nullptr /* mavlink */, &msg); - - if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { - return false; - } - - ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); - ut_compare("Incorrect payload size", reply->size, 1); - ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); - - return true; -} - /// @brief Tests for correct response to an invalid opcpde. bool MavlinkFtpTest::_bad_opcode_test(void) { @@ -191,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { @@ -690,9 +663,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin *payload = reinterpret_cast(ftp_msg->payload); - // Make sure we have a good CRC - ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); - // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); @@ -702,21 +672,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin return true; } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. -uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - payload->padding[2] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} - /// @brief Initializes an FTP message into a mavlink message void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header uint8_t size, ///< size in bytes of data @@ -734,7 +689,8 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, / memcpy(payload->data, data, size); } - payload->crc32 = _payload_crc32(payload); + payload->padding[0] = 0; + payload->padding[1] = 0; msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id @@ -771,7 +727,6 @@ void MavlinkFtpTest::_cleanup_microsd(void) void MavlinkFtpTest::runTests(void) { ut_run_test(_ack_test); - ut_run_test(_bad_crc_test); ut_run_test(_bad_opcode_test); ut_run_test(_bad_datasize_test); ut_run_test(_list_test); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index bbb095a08a..babd909dac 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -66,7 +66,6 @@ public: private: bool _ack_test(void); - bool _bad_crc_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); bool _list_test(void); @@ -81,7 +80,6 @@ private: bool _removefile_test(void); void _receive_message(const mavlink_message_t *msg); - uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, From 0e3c6060b6375fa08b6c0087f7aaf2905ea516d0 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 21:36:08 +0400 Subject: [PATCH 55/64] FTP: remove padding zeroing. --- src/modules/mavlink/mavlink_ftp.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a5c96274be..64206ac544 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,8 +247,6 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->padding[0] = 0; - payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; From 981741f8cea95eebf34577935571260be917abc8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:11:36 -0700 Subject: [PATCH 56/64] Don't send back U or ./.. entries from List command --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++++-- src/modules/mavlink/mavlink_ftp.h | 1 - 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 64206ac544..cf5fae3b3e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -349,11 +349,15 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + continue; + } direntType = kDirentDir; break; default: - direntType = kDirentUnknown; - break; + // We only send back file and diretory entries, skip everything else + continue; } if (entry.d_type == DTYPE_FILE) { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 4892e548c0..b52561e8ab 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,7 +159,6 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command - static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); From dbefa77943fb7f05f391933e82aa4ddf35755851 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:12:04 -0700 Subject: [PATCH 57/64] Update for not getting back "." entries --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 3616db2377..c9566f7db4 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -164,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; + char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { From 46a9f616eba368bb864f4faf8e920dad7fd4ecc5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 12 Sep 2014 10:54:23 -0700 Subject: [PATCH 58/64] Fix List command test Return order from List command is not repeatable --- .../mavlink_tests/mavlink_ftp_test.cpp | 46 ++++++++++++------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index c9566f7db4..022041c74b 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -168,27 +168,20 @@ bool MavlinkFtpTest::_list_test(void) char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { - const char *dir; - char *response; - bool success; + const char *dir; ///< Directory to run List command on + char *response; ///< Expected response entries from List command + int response_count; ///< Number of directories that should be returned + bool success; ///< true: List command should succeed, false: List command should fail }; struct _testCase rgTestCases[] = { - { "/bogus", nullptr, false }, - { "/etc/unit_test_data/mavlink_tests", response1, true }, - { "/", response2, true }, + { "/bogus", nullptr, 0, false }, + { "/etc/unit_test_data/mavlink_tests", response1, 4, true }, + { "/", response2, 4, true }, }; for (size_t i=0; iresponse) + 1; - - char *ptr = strtok (test->response, "|"); - while (ptr != nullptr) - { - ptr = strtok (nullptr, "|"); - } - payload.opcode = MavlinkFTP::kCmdListDirectory; payload.offset = 0; @@ -203,8 +196,29 @@ bool MavlinkFtpTest::_list_test(void) if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Incorrect payload size", reply->size, expected_data_size); - ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); + + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison + // to a hardcoded return result string. + + // Convert null terminators to seperator char so we can use strok to parse returned data + for (uint8_t j=0; jsize-1; j++) { + if (reply->data[j] == 0) { + reply->data[j] = '|'; + } + } + + // Loop over returned directory entries trying to find then in the response list + char *dir; + int response_count = 0; + dir = strtok((char *)&reply->data[0], "|"); + while (dir != nullptr) { + ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); + response_count++; + dir = strtok(nullptr, "|"); + } + + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); From 50f7e27d13e3dfc4b94f17a06f29d775e47627f9 Mon Sep 17 00:00:00 2001 From: Anthony Kenga Date: Mon, 15 Sep 2014 12:24:11 +0300 Subject: [PATCH 59/64] Fixed parameter storage to support struct parameters. --- src/modules/systemlib/param/param.c | 3 ++- src/modules/systemlib/param/param.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e44e6cdb0e..6b8d0e6341 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -322,7 +322,8 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) == PARAM_TYPE_STRUCT) { + if (param_type(param) >= PARAM_TYPE_STRUCT + && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { result = v->p; } else { diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 084cd931a3..dc73b37e91 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -307,7 +307,7 @@ __EXPORT int param_load_default(void); struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default; \ + .val.p = &_default \ } /** From 006717734c351926e8daced9805767970c5e65b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 15 Sep 2014 21:29:19 +0400 Subject: [PATCH 60/64] FTP: Add skip entry information for proper offset calculation. --- src/modules/mavlink/mavlink_ftp.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_ftp.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index cf5fae3b3e..2a85c3702a 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -351,16 +351,20 @@ MavlinkFTP::_workList(PayloadHeader* payload) case DTYPE_DIRECTORY: if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back - continue; + direntType = kDirentSkip; + } else { + direntType = kDirentDir; } - direntType = kDirentDir; break; default: // We only send back file and diretory entries, skip everything else - continue; + direntType = kDirentSkip; } - if (entry.d_type == DTYPE_FILE) { + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + buf[0] = '\0'; + } else if (direntType == kDirentFile) { // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b52561e8ab..657e2f855b 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,6 +159,7 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); From 772c5f745cdb2213e9d89d2ab3f373ac5ea2009a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Sep 2014 16:37:10 -0500 Subject: [PATCH 61/64] Make space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 3 --- 1 file changed, 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2b..6f39b56a13 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -27,8 +27,6 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl @@ -42,7 +40,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/mtd -MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf From ec39427d79249575b1bd8ca7af0929d08c084060 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 19 Sep 2014 09:51:56 +0200 Subject: [PATCH 62/64] geofence: lat/lon is double This is the same change as 5832948371866aec8f0c7f16b13869f270d36aad but against the master branch. Some time ago the type of lat and lon in the global_pos uorb topic was changed but this use here was missed. --- src/modules/navigator/geofence.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2662153084..49aec21e01 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -76,11 +76,7 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; - - return inside(lat, lon, vehicle->alt); + return inside(vehicle->lat, vehicle->lon, vehicle->alt); } bool Geofence::inside(double lat, double lon, float altitude) From ec09f080083fc297228857397eef0651085887d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: [PATCH 63/64] mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f200..7ae7f60cb2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() From 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 64/64] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,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, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; }