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

Merge branch 'master' into offboard2_merge 

Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Julian Oes 2014-07-07 15:11:12 +02:00
parent 79cbf15d26 be73ad0bdb
commit 407eafbdbb
18 changed files with 884 additions and 466 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/commander.cpp
View File

@ -1779,6 +1779,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
} }
print_reject_mode(status, "AUTO_MISSION"); print_reject_mode(status, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} }
// fallback to POSCTL // fallback to POSCTL

2
src/modules/commander/state_machine_helper.cpp
View File

@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
} }
break; break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER: case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */ /* need global position estimate */
if (status->condition_global_position_valid) { if (status->condition_global_position_valid) {
@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
} }
break; break;
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_RTL: case MAIN_STATE_AUTO_RTL:
/* need global position and home position */ /* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) { if (status->condition_global_position_valid && status->condition_home_position_valid) {

616
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -25,6 +25,7 @@ AttPosEKF::AttPosEKF()
useAirspeed = true; useAirspeed = true;
useCompass = true; useCompass = true;
useRangeFinder = true; useRangeFinder = true;
useOpticalFlow = true;
numericalProtection = true; numericalProtection = true;
refSet = false; refSet = false;
storeIndex = 0; storeIndex = 0;
@ -227,10 +228,22 @@ void AttPosEKF::CovariancePrediction(float dt)
// scale gyro bias noise when on ground to allow for faster bias estimation // scale gyro bias noise when on ground to allow for faster bias estimation
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
processNoise[13] = dVelBiasSigma; processNoise[13] = dVelBiasSigma;
for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; if (!inhibitWindStates) {
for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; } else {
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; for (uint8_t i=14; i<=15; i++) processNoise[i] = 0;
}
if (!inhibitMagStates) {
for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
} else {
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
}
if (!inhibitGndHgtState) {
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
} else {
processNoise[22] = 0;
}
// square all sigmas // square all sigmas
for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]);
@ -842,30 +855,6 @@ void AttPosEKF::CovariancePrediction(float dt)
nextP[i][i] = nextP[i][i] + processNoise[i]; nextP[i][i] = nextP[i][i] + processNoise[i];
} }
// If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
// setting the coresponding covariance terms to zero.
if (onGround || !useCompass)
{
zeroRows(nextP,16,21);
zeroCols(nextP,16,21);
}
// If on ground or not using airspeed sensing, inhibit wind velocity
// covariance growth.
if (onGround || !useAirspeed)
{
zeroRows(nextP,14,15);
zeroCols(nextP,14,15);
}
// If on ground, inhibit terrain offset updates by
// setting the coresponding covariance terms to zero.
if (onGround)
{
zeroRows(nextP,22,22);
zeroCols(nextP,22,22);
}
// If the total position variance exceds 1E6 (1000m), then stop covariance // If the total position variance exceds 1E6 (1000m), then stop covariance
// growth by setting the predicted to the previous values // growth by setting the predicted to the previous values
// This prevent an ill conditioned matrix from occurring for long periods // This prevent an ill conditioned matrix from occurring for long periods
@ -882,48 +871,22 @@ void AttPosEKF::CovariancePrediction(float dt)
} }
} }
if (onGround || staticMode) { // Copy covariance
// copy the portion of the variances we want to for (unsigned i = 0; i < n_states; i++) {
// propagate P[i][i] = nextP[i][i];
for (unsigned i = 0; i <= 13; i++) {
P[i][i] = nextP[i][i];
}
// force symmetry for observable states
// force zero for non-observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
if ((i > 13) || (j > 13)) {
P[i][j] = 0.0f;
} else {
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
}
P[j][i] = P[i][j];
}
}
} else {
// Copy covariance
for (unsigned i = 0; i < n_states; i++) {
P[i][i] = nextP[i][i];
}
// force symmetry for observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
}
} }
ConstrainVariances(); // force symmetry for observable states
for (unsigned i = 1; i < n_states; i++)
{
for (uint8_t j = 0; j < i; j++)
{
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
}
ConstrainVariances();
} }
void AttPosEKF::FuseVelposNED() void AttPosEKF::FuseVelposNED()
@ -944,7 +907,7 @@ void AttPosEKF::FuseVelposNED()
bool fuseData[6] = {false,false,false,false,false,false}; bool fuseData[6] = {false,false,false,false,false,false};
uint8_t stateIndex; uint8_t stateIndex;
uint8_t obsIndex; uint8_t obsIndex;
uint8_t indexLimit; uint8_t indexLimit = 22;
// declare variables used by state and covariance update calculations // declare variables used by state and covariance update calculations
float velErr; float velErr;
@ -981,11 +944,6 @@ void AttPosEKF::FuseVelposNED()
R_OBS[4] = R_OBS[3]; R_OBS[4] = R_OBS[3];
R_OBS[5] = sq(posDSigma) + sq(posErr); R_OBS[5] = sq(posDSigma) + sq(posErr);
// Set innovation variances to zero default
for (uint8_t i = 0; i<=5; i++)
{
varInnovVelPos[i] = 0.0f;
}
// calculate innovations and check GPS data validity using an innovation consistency check // calculate innovations and check GPS data validity using an innovation consistency check
if (fuseVelData) if (fuseVelData)
{ {
@ -1071,15 +1029,6 @@ void AttPosEKF::FuseVelposNED()
{ {
fuseData[5] = true; fuseData[5] = true;
} }
// Limit range of states modified when on ground
if(!onGround)
{
indexLimit = 22;
}
else
{
indexLimit = 13;
}
// Fuse measurements sequentially // Fuse measurements sequentially
for (obsIndex=0; obsIndex<=5; obsIndex++) for (obsIndex=0; obsIndex<=5; obsIndex++)
{ {
@ -1113,6 +1062,22 @@ void AttPosEKF::FuseVelposNED()
if (obsIndex != 5) { if (obsIndex != 5) {
Kfusion[13] = 0; Kfusion[13] = 0;
} }
// Don't update wind states if inhibited
if (inhibitWindStates) {
Kfusion[14] = 0;
Kfusion[15] = 0;
}
// Don't update magnetic field states if inhibited
if (inhibitMagStates) {
for (uint8_t i = 16; i<=21; i++)
{
Kfusion[i] = 0;
}
}
// Don't update terrain state if inhibited
if (inhibitGndHgtState) {
Kfusion[22] = 0;
}
// Calculate state corrections and re-normalise the quaternions // Calculate state corrections and re-normalise the quaternions
for (uint8_t i = 0; i<=indexLimit; i++) for (uint8_t i = 0; i<=indexLimit; i++)
@ -1179,7 +1144,6 @@ void AttPosEKF::FuseMagnetometer()
for (uint8_t i = 0; i < n_states; i++) { for (uint8_t i = 0; i < n_states; i++) {
H_MAG[i] = 0.0f; H_MAG[i] = 0.0f;
} }
unsigned indexLimit;
// Perform sequential fusion of Magnetometer measurements. // Perform sequential fusion of Magnetometer measurements.
// This assumes that the errors in the different components are // This assumes that the errors in the different components are
@ -1189,19 +1153,6 @@ void AttPosEKF::FuseMagnetometer()
// associated with sequential fusion // associated with sequential fusion
if (useCompass && fuseMagData && (obsIndex < 3)) if (useCompass && fuseMagData && (obsIndex < 3))
{ {
// Limit range of states modified when on ground
if(!onGround)
{
indexLimit = n_states;
}
else
{
indexLimit = 13 + 1;
}
// Sequential fusion of XYZ components to spread processing load across
// three prediction time steps.
// Calculate observation jacobians and Kalman gains // Calculate observation jacobians and Kalman gains
if (obsIndex == 0) if (obsIndex == 0)
{ {
@ -1287,15 +1238,31 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); // Estimation of selected states is inhibited by setting their Kalman gains to zero
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); if (!inhibitWindStates) {
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); } else {
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); Kfusion[14] = 0;
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[15] = 0;
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); }
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); if (!inhibitMagStates) {
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
} else {
for (uint8_t i=16; i <= 21; i++) {
Kfusion[i] = 0;
}
}
if (!inhibitGndHgtState) {
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;
}
varInnovMag[0] = 1.0f/SK_MX[0]; varInnovMag[0] = 1.0f/SK_MX[0];
innovMag[0] = MagPred[0] - magData.x; innovMag[0] = MagPred[0] - magData.x;
} }
@ -1342,15 +1309,34 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); // Estimation of selected states is inhibited by setting their Kalman gains to zero
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); if (!inhibitWindStates) {
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); } else {
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); Kfusion[14] = 0;
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); Kfusion[15] = 0;
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); }
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]); if (!inhibitMagStates) {
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
} else {
Kfusion[16] = 0;
Kfusion[17] = 0;
Kfusion[18] = 0;
Kfusion[19] = 0;
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]; varInnovMag[1] = 1.0f/SK_MY[0];
innovMag[1] = MagPred[1] - magData.y; innovMag[1] = MagPred[1] - magData.y;
} }
@ -1398,15 +1384,34 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); // Estimation of selected states is inhibited by setting their Kalman gains to zero
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); if (!inhibitWindStates) {
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); } else {
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); Kfusion[14] = 0;
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); Kfusion[15] = 0;
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); }
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]); if (!inhibitMagStates) {
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
} else {
Kfusion[16] = 0;
Kfusion[17] = 0;
Kfusion[18] = 0;
Kfusion[19] = 0;
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]; varInnovMag[2] = 1.0f/SK_MZ[0];
innovMag[2] = MagPred[2] - magData.z; innovMag[2] = MagPred[2] - magData.z;
@ -1416,7 +1421,7 @@ void AttPosEKF::FuseMagnetometer()
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
{ {
// correct the state vector // correct the state vector
for (uint8_t j= 0; j < indexLimit; j++) for (uint8_t j= 0; j < n_states; j++)
{ {
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
} }
@ -1433,7 +1438,7 @@ void AttPosEKF::FuseMagnetometer()
// correct the covariance P = (I - K*H)*P // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // take advantage of the empty columns in KH to reduce the
// number of operations // number of operations
for (uint8_t i = 0; i < indexLimit; i++) for (uint8_t i = 0; i < n_states; i++)
{ {
for (uint8_t j = 0; j <= 3; j++) for (uint8_t j = 0; j <= 3; j++)
{ {
@ -1455,9 +1460,9 @@ void AttPosEKF::FuseMagnetometer()
} }
} }
} }
for (uint8_t i = 0; i < indexLimit; i++) for (uint8_t i = 0; i < n_states; i++)
{ {
for (uint8_t j = 0; j < indexLimit; j++) for (uint8_t j = 0; j < n_states; j++)
{ {
KHP[i][j] = 0.0f; KHP[i][j] = 0.0f;
for (uint8_t k = 0; k <= 3; k++) for (uint8_t k = 0; k <= 3; k++)
@ -1474,9 +1479,9 @@ void AttPosEKF::FuseMagnetometer()
} }
} }
} }
for (uint8_t i = 0; i < indexLimit; i++) for (uint8_t i = 0; i < n_states; i++)
{ {
for (uint8_t j = 0; j < indexLimit; j++) for (uint8_t j = 0; j < n_states; j++)
{ {
P[i][j] = P[i][j] - KHP[i][j]; P[i][j] = P[i][j] - KHP[i][j];
} }
@ -1552,15 +1557,31 @@ void AttPosEKF::FuseAirspeed()
Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); // Estimation of selected states is inhibited by setting their Kalman gains to zero
Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); if (!inhibitWindStates) {
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); } else {
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); Kfusion[14] = 0;
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[15] = 0;
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); }
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]); if (!inhibitMagStates) {
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
} else {
for (uint8_t i=16; i <= 21; i++) {
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; varInnovVtas = 1.0f/SK_TAS;
// Calculate the measurement innovation // Calculate the measurement innovation
@ -1662,9 +1683,9 @@ void AttPosEKF::FuseRangeFinder()
float ptd = statesAtRngTime[22]; float ptd = statesAtRngTime[22];
// Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
SH_RNG[4] = sin(rngFinderPitch); SH_RNG[4] = sinf(rngFinderPitch);
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
if (useRangeFinder && cosRngTilt > 0.87f) if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f)
{ {
// Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
// This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations
@ -1685,10 +1706,12 @@ void AttPosEKF::FuseRangeFinder()
SK_RNG[5] = SH_RNG[2]; SK_RNG[5] = SH_RNG[2];
Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]);
// Calculate the innovation variance for data logging
varInnovRng = 1.0f/SK_RNG[0];
// Calculate the measurement innovation // Calculate the measurement innovation
rngPred = (ptd - pd)/cosRngTilt; rngPred = (ptd - pd)/cosRngTilt;
innovRng = rngPred - rngMea; innovRng = rngPred - rngMea;
//printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd);
// Check the innovation for consistency and don't fuse if > 5Sigma // Check the innovation for consistency and don't fuse if > 5Sigma
if ((innovRng*innovRng*SK_RNG[0]) < 25) if ((innovRng*innovRng*SK_RNG[0]) < 25)
@ -1704,6 +1727,293 @@ 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 Vector3f delAng;
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;
}
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;
// calculate delta angles in sensor axes
Vector3f delAngRel = Tbs*delAng;
// 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];
// reset the observation index to 0 (we start by fusing the X
// measurement)
obsIndex = 0;
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];
}
// 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-12)
{
for (uint8_t j= 0; j<=3; j++)
{
float quatMagInv = 1.0f/quatMag;
states[j] = states[j] * quatMagInv;
}
}
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j <= 6; j++)
{
KH[i][j] = Kfusion[i] * H_LOS[j];
}
for (uint8_t j = 7; j <= 8; j++)
{
KH[i][j] = 0.0f;
}
KH[i][9] = Kfusion[i] * H_LOS[9];
for (uint8_t j = 10; j <= 21; j++)
{
KH[i][j] = 0.0f;
}
KH[i][22] = Kfusion[i] * H_LOS[22];
}
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j < n_states; j++)
{
KHP[i][j] = 0.0f;
for (uint8_t k = 0; k <= 6; k++)
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
}
}
}
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j < n_states; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
}
}
}
obsIndex = obsIndex + 1;
ForceSymmetry();
ConstrainVariances();
}
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
{ {
uint8_t row; uint8_t row;
@ -1904,6 +2214,24 @@ void AttPosEKF::OnGroundCheck()
if (staticMode) { if (staticMode) {
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
} }
// don't update wind states if there is no airspeed measurement
if (onGround || !useAirspeed) {
inhibitWindStates = true;
} else {
inhibitWindStates =false;
}
// don't update magnetic field states if on ground or not using compass
if (onGround || !useCompass) {
inhibitMagStates = true;
} else {
inhibitMagStates = false;
}
// don't update terrain offset state if on ground
if (onGround) {
inhibitGndHgtState = true;
} else {
inhibitGndHgtState = false;
}
} }
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
@ -1931,8 +2259,8 @@ void AttPosEKF::CovarianceInit()
P[11][11] = P[10][10]; P[11][11] = P[10][10];
P[12][12] = P[10][10]; P[12][12] = P[10][10];
P[13][13] = sq(0.2f*dtIMU); P[13][13] = sq(0.2f*dtIMU);
P[14][14] = sq(8.0f); P[14][14] = sq(0.0f);
P[15][14] = P[14][14]; P[15][15] = P[14][14];
P[16][16] = sq(0.02f); P[16][16] = sq(0.02f);
P[17][17] = P[16][16]; P[17][17] = P[16][16];
P[18][18] = P[16][16]; P[18][18] = P[16][16];

20
src/modules/ekf_att_pos_estimator/estimator_23states.h
View File

@ -29,6 +29,10 @@ public:
float covDelAngMax; // maximum delta angle between covariance predictions float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float a1; // optical flow sensor misalgnment angle about X axis (rad)
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
float yawVarScale; float yawVarScale;
float windVelSigma; float windVelSigma;
float dAngBiasSigma; float dAngBiasSigma;
@ -55,6 +59,9 @@ public:
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f; EAS2TAS = 1.0f;
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
yawVarScale = 1.0f; yawVarScale = 1.0f;
windVelSigma = 0.1f; windVelSigma = 0.1f;
@ -115,6 +122,7 @@ public:
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states 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 statesAtRngTime[n_states]; // filter states at the effective measurement time
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@ -147,9 +155,13 @@ public:
float innovMag[3]; // innovation output float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float losData[2]; // optical flow LOS rate measurements (rad/sec)
float innovVtas; // innovation output float innovVtas; // innovation output
float innovRng; ///< Range finder innovation float innovRng; ///< Range finder innovation
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
float varInnovVtas; // innovation variance output float varInnovVtas; // innovation variance output
float varInnovRng; // range finder innovation variance
float VtasMeas; // true airspeed measurement (m/s) float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad) double latRef; // WGS-84 latitude of reference point (rad)
@ -178,12 +190,18 @@ public:
bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused bool fuseRngData; ///< true when range data is fused
bool fuseOptFlowData; // true when optical flow data is fused
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 onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used bool useRangeFinder; ///< true when rangefinder is being used
bool useOpticalFlow; // true when optical flow data is being used
bool ekfDiverged; bool ekfDiverged;
uint64_t lastReset; uint64_t lastReset;
@ -208,7 +226,7 @@ void FuseAirspeed();
void FuseRangeFinder(); void FuseRangeFinder();
void FuseOpticalFlow(); void FuseOptFlow();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);

20
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -8,6 +8,7 @@
#ifdef EKF_DEBUG #ifdef EKF_DEBUG
#include <stdio.h> #include <stdio.h>
#include <stdarg.h>
static void static void
ekf_debug_print(const char *fmt, va_list args) ekf_debug_print(const char *fmt, va_list args)
@ -101,6 +102,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn)
return vecOut; return vecOut;
} }
// overload * operator to provide a matrix product
Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
{
Mat3f matOut;
matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
return matOut;
}
// overload % operator to provide a vector cross product // overload % operator to provide a vector cross product
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
{ {

3
src/modules/ekf_att_pos_estimator/estimator_utilities.h
View File

@ -41,6 +41,7 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1);
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*( Mat3f matIn, Vector3f vecIn); Vector3f operator*( Mat3f matIn, Vector3f vecIn);
Mat3f operator*( Mat3f matIn1, Mat3f matIn2);
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
Vector3f operator*(Vector3f vecIn1, float sclIn1); Vector3f operator*(Vector3f vecIn1, float sclIn1);
@ -79,4 +80,4 @@ struct ekf_status_report {
bool velOffsetExcessive; bool velOffsetExcessive;
}; };
void ekf_debug(const char *fmt, ...); void ekf_debug(const char *fmt, ...);

33
src/modules/navigator/loiter.cpp
View File

@ -36,6 +36,7 @@
* Helper class to loiter * Helper class to loiter
* *
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <string.h> #include <string.h>
@ -51,28 +52,42 @@
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
#include "loiter.h" #include "loiter.h"
#include "navigator.h"
Loiter::Loiter(Navigator *navigator, const char *name) : Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name) MissionBlock(navigator, name)
{ {
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* initial reset */
on_inactive();
} }
Loiter::~Loiter() Loiter::~Loiter()
{ {
} }
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(pos_sp_triplet);
}
void void
Loiter::on_inactive() Loiter::on_inactive()
{ {
} }
void
Loiter::on_activation()
{
/* set current mission item to loiter */
set_loiter_item(&_mission_item);
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
}
void
Loiter::on_active()
{
}

16
src/modules/navigator/loiter.h
View File

@ -50,25 +50,15 @@
class Loiter : public MissionBlock class Loiter : public MissionBlock
{ {
public: public:
/**
* Constructor
*/
Loiter(Navigator *navigator, const char *name); Loiter(Navigator *navigator, const char *name);
/**
* Destructor
*/
~Loiter(); ~Loiter();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive(); virtual void on_inactive();
/** virtual void on_activation();
* This function is called while the mode is active
*/ virtual void on_active();
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
}; };
#endif #endif

319
src/modules/navigator/mission.cpp
View File

@ -54,25 +54,28 @@
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h> #include <uORB/topics/mission_result.h>
#include "navigator.h"
#include "mission.h" #include "mission.h"
#include "navigator.h"
Mission::Mission(Navigator *navigator, const char *name) : Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name), MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"), _param_onboard_enabled(this, "ONBOARD_EN"),
_param_takeoff_alt(this, "TAKEOFF_ALT"),
_onboard_mission({0}), _onboard_mission({0}),
_offboard_mission({0}), _offboard_mission({0}),
_current_onboard_mission_index(-1), _current_onboard_mission_index(-1),
_current_offboard_mission_index(-1), _current_offboard_mission_index(-1),
_mission_result_pub(-1), _mission_result_pub(-1),
_mission_result({0}), _mission_result({0}),
_mission_type(MISSION_TYPE_NONE) _mission_type(MISSION_TYPE_NONE),
_need_takeoff(true),
_takeoff(false)
{ {
/* load initial params */ /* load initial params */
updateParams(); updateParams();
/* set initial mission items */ /* set initial mission items */
on_inactive(); on_inactive();
} }
Mission::~Mission() Mission::~Mission()
@ -82,8 +85,6 @@ Mission::~Mission()
void void
Mission::on_inactive() Mission::on_inactive()
{ {
_first_run = true;
/* check anyway if missions have changed so that feedback to groundstation is given */ /* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated; bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -96,13 +97,21 @@ Mission::on_inactive()
if (offboard_updated) { if (offboard_updated) {
update_offboard_mission(); update_offboard_mission();
} }
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
} }
bool void
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) Mission::on_activation()
{ {
bool updated = false; set_mission_items();
}
void
Mission::on_active()
{
/* check if anything has changed */ /* check if anything has changed */
bool onboard_updated; bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@ -117,20 +126,21 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
} }
/* reset mission items if needed */ /* reset mission items if needed */
if (onboard_updated || offboard_updated || _first_run) { if (onboard_updated || offboard_updated) {
set_mission_items(pos_sp_triplet); set_mission_items();
updated = true;
_first_run = false;
} }
/* lets check if we reached the current mission item */ /* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission(); advance_mission();
set_mission_items(pos_sp_triplet); set_mission_items();
updated = true;
}
return updated; } else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
_navigator->set_can_loiter_at_sp(true);
}
}
} }
void void
@ -151,6 +161,7 @@ Mission::update_onboard_mission()
} }
/* otherwise, just leave it */ /* otherwise, just leave it */
} }
} else { } else {
_onboard_mission.count = 0; _onboard_mission.count = 0;
_onboard_mission.current_index = 0; _onboard_mission.current_index = 0;
@ -167,10 +178,12 @@ Mission::update_offboard_mission()
if (_offboard_mission.current_index >= 0 if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) { && _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index; _current_offboard_mission_index = _offboard_mission.current_index;
} else { } else {
/* if less WPs available, reset to first WP */ /* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) { if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0; _current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */ /* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) { } else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0; _current_offboard_mission_index = 0;
@ -184,6 +197,7 @@ Mission::update_offboard_mission()
if (_offboard_mission.dataman_id == 0) { if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else { } else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
} }
@ -192,11 +206,13 @@ Mission::update_offboard_mission()
(size_t)_offboard_mission.count, (size_t)_offboard_mission.count,
_navigator->get_geofence(), _navigator->get_geofence(),
_navigator->get_home_position()->alt); _navigator->get_home_position()->alt);
} else { } else {
_offboard_mission.count = 0; _offboard_mission.count = 0;
_offboard_mission.current_index = 0; _offboard_mission.current_index = 0;
_current_offboard_mission_index = 0; _current_offboard_mission_index = 0;
} }
report_current_offboard_mission_item(); report_current_offboard_mission_item();
} }
@ -204,46 +220,55 @@ Mission::update_offboard_mission()
void void
Mission::advance_mission() Mission::advance_mission()
{ {
switch (_mission_type) { if (_takeoff) {
case MISSION_TYPE_ONBOARD: _takeoff = false;
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD: } else {
_current_offboard_mission_index++; switch (_mission_type) {
break; case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_NONE: case MISSION_TYPE_OFFBOARD:
default: _current_offboard_mission_index++;
break; break;
case MISSION_TYPE_NONE:
default:
break;
}
} }
} }
void void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) Mission::set_mission_items()
{ {
set_previous_pos_setpoint(pos_sp_triplet); /* make sure param is up to date */
updateParams();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* try setting onboard mission item */ /* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */ /* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) { if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
"#audio: onboard mission running");
} }
_mission_type = MISSION_TYPE_ONBOARD; _mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */ /* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { } else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */ /* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) { if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
"#audio: offboard mission running");
} }
_mission_type = MISSION_TYPE_OFFBOARD; _mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_can_loiter_at_sp(false);
} else { } else {
/* no mission available, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) { if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(), mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished"); "#audio: mission finished");
@ -252,125 +277,152 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
"#audio: no mission available"); "#audio: no mission available");
} }
_mission_type = MISSION_TYPE_NONE; _mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
set_loiter_item(pos_sp_triplet); /* set loiter mission item */
set_loiter_item(&_mission_item);
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached(); reset_mission_item_reached();
report_mission_finished(); report_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
return;
} }
}
bool /* do takeoff on first waypoint for rotary wing vehicles */
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) if (_navigator->get_vstatus()->is_rotary_wing) {
{ /* force takeoff if landed (additional protection) */
/* make sure param is up to date */ if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
updateParams(); _need_takeoff = true;
if (_param_onboard_enabled.get() > 0 && }
_current_onboard_mission_index >= 0&&
_current_onboard_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
&new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached(); /* new current mission item set, check if we need takeoff */
if (_need_takeoff && (
/* TODO: report this somehow */ _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
return true; _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_takeoff = true;
_need_takeoff = false;
} }
} }
return false;
}
bool if (_takeoff) {
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) /* do takeoff before going to setpoint */
{ /* set mission item as next position setpoint */
if (_current_offboard_mission_index >= 0 && mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
_current_offboard_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current; /* calculate takeoff altitude */
if (_offboard_mission.dataman_id == 0) { float takeoff_alt = _mission_item.altitude;
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; if (_mission_item.altitude_is_relative) {
takeoff_alt += _navigator->get_home_position()->alt;
}
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else { } else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
} }
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached(); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt);
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
} else {
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item(); report_current_offboard_mission_item();
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
} }
} // TODO: report onboard mission item somehow
return false;
}
void /* try to read next mission item */
Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) struct mission_item_s mission_item_next;
{
int next_temp_mission_index = _onboard_mission.current_index + 1;
/* try if there is a next onboard mission */ if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
if (_onboard_mission.current_index >= 0 && /* got next mission item, update setpoint triplet */
next_temp_mission_index < (int)_onboard_mission.count) { mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
void
Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
/* try if there is a next offboard mission */
int next_temp_mission_index = _offboard_mission.current_index + 1;
warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
if (_offboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else { } else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; /* next mission item is not available */
} pos_sp_triplet->next.valid = false;
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
} }
} }
/* give up */
next_pos_sp->valid = false; _navigator->set_position_setpoint_triplet_updated();
return;
} }
bool bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
struct mission_item_s *new_mission_item)
{ {
/* select onboard/offboard mission */
int *mission_index_ptr;
struct mission_s *mission;
dm_item_t dm_item;
int mission_index_next;
if (onboard) {
/* onboard mission */
mission_index_next = _current_onboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
mission = &_onboard_mission;
dm_item = DM_KEY_WAYPOINTS_ONBOARD;
} else {
/* offboard mission */
mission_index_next = _current_offboard_mission_index + 1;
mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
mission = &_offboard_mission;
if (_offboard_mission.dataman_id == 0) {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
}
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
return false;
}
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) { for (int i = 0; i < 10; i++) {
const ssize_t len = sizeof(struct mission_item_s); const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
struct mission_item_s mission_item_tmp;
/* read mission item from datamanager */ /* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */ /* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(), mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read"); "#audio: ERROR waypoint could not be read");
@ -378,18 +430,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
} }
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
/* do DO_JUMP as many times as requested */ /* do DO_JUMP as many times as requested */
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item /* only raise the repeat count if this is for the current mission item
* but not for the next mission item */ * but not for the next mission item */
if (is_current) { if (is_current) {
(new_mission_item->do_jump_current_count)++; (mission_item_tmp.do_jump_current_count)++;
/* save repeat count */ /* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the /* not supposed to happen unless the datamanager can't access the
* dataman */ * dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(), mavlink_log_critical(_navigator->get_mavlink_fd(),
@ -399,16 +450,18 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
} }
/* set new mission item index and repeat /* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/ * we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index; *mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else { } else {
mavlink_log_info(_navigator->get_mavlink_fd(), mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed"); "#audio: DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */ /* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index)++; (*mission_index_ptr)++;
} }
} else { } else {
/* if it's not a DO_JUMP, then we were successful */ /* if it's not a DO_JUMP, then we were successful */
memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
return true; return true;
} }
} }

48
src/modules/navigator/mission.h
View File

@ -65,25 +65,15 @@ class Navigator;
class Mission : public MissionBlock class Mission : public MissionBlock
{ {
public: public:
/**
* Constructor
*/
Mission(Navigator *navigator, const char *name); Mission(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~Mission(); virtual ~Mission();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive(); virtual void on_inactive();
/** virtual void on_activation();
* This function is called while the mode is active
*/ virtual void on_active();
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
private: private:
/** /**
@ -104,36 +94,13 @@ private:
/** /**
* Set new mission items * Set new mission items
*/ */
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); void set_mission_items();
/** /**
* Try to set the current position setpoint from an onboard mission item * Read current or next mission item from the dataman and watch out for DO_JUMPS
* @return true if mission item successfully set
*/
bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the current position setpoint from an offboard mission item
* @return true if mission item successfully set
*/
bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the next position setpoint from an onboard mission item
*/
void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Try to set the next position setpoint from an offboard mission item
*/
void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Read a mission item from the dataman and watch out for DO_JUMPS
* @return true if successful * @return true if successful
*/ */
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
struct mission_item_s *new_mission_item);
/** /**
* Report that a mission item has been reached * Report that a mission item has been reached
@ -156,12 +123,15 @@ private:
void publish_mission_result(); void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled; control::BlockParamFloat _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
struct mission_s _onboard_mission; struct mission_s _onboard_mission;
struct mission_s _offboard_mission; struct mission_s _offboard_mission;
int _current_onboard_mission_index; int _current_onboard_mission_index;
int _current_offboard_mission_index; int _current_offboard_mission_index;
bool _need_takeoff;
bool _takeoff;
orb_advert_t _mission_result_pub; orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result; struct mission_result_s _mission_result;

75
src/modules/navigator/mission_block.cpp
View File

@ -47,6 +47,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h> #include <geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
@ -59,8 +60,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
_waypoint_position_reached(false), _waypoint_position_reached(false),
_waypoint_yaw_reached(false), _waypoint_yaw_reached(false),
_time_first_inside_orbit(0), _time_first_inside_orbit(0),
_mission_item({0}), _mission_item({0})
_mission_item_valid(false)
{ {
} }
@ -71,6 +71,10 @@ MissionBlock::~MissionBlock()
bool bool
MissionBlock::is_mission_item_reached() MissionBlock::is_mission_item_reached()
{ {
if (_mission_item.nav_cmd == NAV_CMD_IDLE) {
return false;
}
if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _navigator->get_vstatus()->condition_landed; return _navigator->get_vstatus()->condition_landed;
} }
@ -84,7 +88,6 @@ MissionBlock::is_mission_item_reached()
hrt_abstime now = hrt_absolute_time(); hrt_abstime now = hrt_absolute_time();
if (!_waypoint_position_reached) { if (!_waypoint_position_reached) {
float dist = -1.0f; float dist = -1.0f;
float dist_xy = -1.0f; float dist_xy = -1.0f;
float dist_z = -1.0f; float dist_z = -1.0f;
@ -201,44 +204,48 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
} }
void void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) MissionBlock::set_previous_pos_setpoint()
{ {
/* reuse current setpoint as previous setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (pos_sp_triplet->current.valid) { if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
} }
} }
bool void
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) MissionBlock::set_loiter_item(struct mission_item_s *item)
{ {
/* don't change setpoint if 'can_loiter_at_sp' flag set */ if (_navigator->get_vstatus()->condition_landed) {
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { /* landed, don't takeoff, but switch to IDLE mode */
/* use current position */ item->nav_cmd = NAV_CMD_IDLE;
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator->set_can_loiter_at_sp(true); } else {
item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
/* use current position setpoint */
item->lat = pos_sp_triplet->current.lat;
item->lon = pos_sp_triplet->current.lon;
item->altitude = pos_sp_triplet->current.alt;
} else {
/* use current position */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
}
item->altitude_is_relative = false;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->pitch_min = 0.0f;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
} }
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
pos_sp_triplet->current.loiter_direction = 1;
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
return true;
}
return false;
} }

14
src/modules/navigator/mission_block.h
View File

@ -64,6 +64,7 @@ public:
*/ */
virtual ~MissionBlock(); virtual ~MissionBlock();
protected:
/** /**
* Check if mission item has been reached * Check if mission item has been reached
* @return true if successfully reached * @return true if successfully reached
@ -85,22 +86,17 @@ public:
/** /**
* Set previous position setpoint to current setpoint * Set previous position setpoint to current setpoint
*/ */
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); void set_previous_pos_setpoint();
/** /**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/ */
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); void set_loiter_item(struct mission_item_s *item);
mission_item_s _mission_item;
bool _waypoint_position_reached; bool _waypoint_position_reached;
bool _waypoint_yaw_reached; bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit; hrt_abstime _time_first_inside_orbit;
mission_item_s _mission_item;
bool _mission_item_valid;
}; };
#endif #endif

3
src/modules/navigator/navigator.h
View File

@ -104,6 +104,7 @@ public:
* Setters * Setters
*/ */
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
/** /**
* Getters * Getters
@ -168,7 +169,7 @@ private:
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */

15
src/modules/navigator/navigator_main.cpp
View File

@ -124,7 +124,7 @@ Navigator::Navigator() :
_loiter(this, "LOI"), _loiter(this, "LOI"),
_rtl(this, "RTL"), _rtl(this, "RTL"),
_offboard(this, "OFF"), _offboard(this, "OFF"),
_update_triplet(false), _update_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"), _param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD") _param_acceptance_radius(this, "ACC_RAD")
{ {
@ -379,24 +379,21 @@ Navigator::task_main()
/* iterate through navigation modes and set active/inactive for each */ /* iterate through navigation modes and set active/inactive for each */
for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
if (_navigation_mode == _navigation_mode_array[i]) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
} else {
_navigation_mode_array[i]->on_inactive();
}
} }
/* if nothing is running, set position setpoint triplet invalid */ /* if nothing is running, set position setpoint triplet invalid */
if (_navigation_mode == nullptr) { if (_navigation_mode == nullptr) {
// TODO publish empty sp only once
_pos_sp_triplet.previous.valid = false; _pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false; _pos_sp_triplet.next.valid = false;
_update_triplet = true; _pos_sp_triplet_updated = true;
} }
if (_update_triplet) { if (_pos_sp_triplet_updated) {
publish_position_setpoint_triplet(); publish_position_setpoint_triplet();
_update_triplet = false; _pos_sp_triplet_updated = false;
} }
perf_end(_loop_perf); perf_end(_loop_perf);

43
src/modules/navigator/navigator_mode.cpp
View File

@ -33,12 +33,14 @@
/** /**
* @file navigator_mode.cpp * @file navigator_mode.cpp
* *
* Helper class for different modes in navigator * Base class for different modes in navigator
* *
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include "navigator_mode.h" #include "navigator_mode.h"
#include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name), SuperBlock(NULL, name),
@ -56,15 +58,38 @@ NavigatorMode::~NavigatorMode()
} }
void void
NavigatorMode::on_inactive() NavigatorMode::run(bool active) {
{ if (active) {
_first_run = true; if (_first_run) {
/* first run */
_first_run = false;
on_activation();
} else {
/* periodic updates when active */
on_active();
}
} else {
/* periodic updates when inactive */
_first_run = true;
on_inactive();
}
} }
bool void
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) NavigatorMode::on_inactive()
{
}
void
NavigatorMode::on_activation()
{
/* invalidate position setpoint by default */
_navigator->get_position_setpoint_triplet()->current.valid = false;
}
void
NavigatorMode::on_active()
{ {
pos_sp_triplet->current.valid = false;
_first_run = false;
return false;
} }

19
src/modules/navigator/navigator_mode.h
View File

@ -33,9 +33,10 @@
/** /**
* @file navigator_mode.h * @file navigator_mode.h
* *
* Helper class for different modes in navigator * Base class for different modes in navigator
* *
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#ifndef NAVIGATOR_MODE_H #ifndef NAVIGATOR_MODE_H
@ -65,21 +66,27 @@ public:
*/ */
virtual ~NavigatorMode(); virtual ~NavigatorMode();
void run(bool active);
/** /**
* This function is called while the mode is inactive * This function is called while the mode is inactive
*/ */
virtual void on_inactive(); virtual void on_inactive();
/** /**
* This function is called while the mode is active * This function is called one time when mode become active, poos_sp_triplet must be initialized here
*
* @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed
*/ */
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); virtual void on_activation();
/**
* This function is called while the mode is active
*/
virtual void on_active();
protected: protected:
Navigator *_navigator; Navigator *_navigator;
private:
bool _first_run; bool _first_run;
}; };

75
src/modules/navigator/rtl.cpp
View File

@ -75,62 +75,57 @@ RTL::~RTL()
void void
RTL::on_inactive() RTL::on_inactive()
{ {
_first_run = true;
/* reset RTL state only if setpoint moved */ /* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) { if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE; _rtl_state = RTL_STATE_NONE;
} }
} }
bool void
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) RTL::on_activation()
{ {
bool updated = false; /* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
if (_first_run) { /* if lower than return altitude, climb up first */
_first_run = false; } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* decide where to enter the RTL procedure when we switch into it */ /* otherwise go straight to return */
if (_rtl_state == RTL_STATE_NONE) { } else {
/* for safety reasons don't go into RTL if landed */ /* set altitude setpoint to current altitude */
if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_RETURN;
_rtl_state = RTL_STATE_LANDED; _mission_item.altitude_is_relative = false;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); _mission_item.altitude = _navigator->get_global_position()->alt;
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
} }
set_rtl_item(pos_sp_triplet);
updated = true;
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
} }
return updated; set_rtl_item();
} }
void void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) RTL::on_active()
{ {
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item();
}
}
void
RTL::set_rtl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* make sure we have the latest params */ /* make sure we have the latest params */
updateParams(); updateParams();
set_previous_pos_setpoint(pos_sp_triplet); set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false); _navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) { switch (_rtl_state) {
@ -277,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
break; break;
} }
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */ /* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false; pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
} }
void void

22
src/modules/navigator/rtl.h
View File

@ -57,35 +57,21 @@ class Navigator;
class RTL : public MissionBlock class RTL : public MissionBlock
{ {
public: public:
/**
* Constructor
*/
RTL(Navigator *navigator, const char *name); RTL(Navigator *navigator, const char *name);
/**
* Destructor
*/
~RTL(); ~RTL();
/** virtual void on_inactive();
* This function is called while the mode is inactive
*/
void on_inactive();
/** virtual void on_activation();
* This function is called while the mode is active
*
* @param position setpoint triplet that needs to be set
* @return true if updated
*/
bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
virtual void on_active();
private: private:
/** /**
* Set the RTL item * Set the RTL item
*/ */
void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); void set_rtl_item();
/** /**
* Move to next RTL item * Move to next RTL item