zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF3: constify array index variable

zr-v5.1
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
1498b516a3
commit
ac87cab6bc
6 changed files with 13 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
  2. 8
      libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
  3. 4
      libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
  4. 6
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
  5. 2
      libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
  6. 2
      libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
Unescape View File

@ -95,7 +95,7 @@ void NavEKF3_core::FuseAirspeed() @@ -95,7 +95,7 @@ void NavEKF3_core::FuseAirspeed()
if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = SK_TAS[0]*(P[stateIndex][4]*SH_TAS[2] - P[stateIndex][22]*SH_TAS[2] + P[stateIndex][5]*SK_TAS[1] - P[stateIndex][23]*SK_TAS[1] + P[stateIndex][6]*vd*SH_TAS[0]);
} else {
@ -394,7 +394,7 @@ void NavEKF3_core::FuseSideslip() @@ -394,7 +394,7 @@ void NavEKF3_core::FuseSideslip()
if (!inhibitDelVelBiasStates && !airDataFusionWindOnly) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = SK_BETA[0]*(P[stateIndex][0]*SK_BETA[5] + P[stateIndex][1]*SK_BETA[4] - P[stateIndex][4]*SK_BETA[1] + P[stateIndex][5]*SK_BETA[2] + P[stateIndex][2]*SK_BETA[6] + P[stateIndex][6]*SK_BETA[3] - P[stateIndex][3]*SK_BETA[7] + P[stateIndex][22]*SK_BETA[1] - P[stateIndex][23]*SK_BETA[2]);
} else {

8
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
Unescape View File

@ -602,7 +602,7 @@ void NavEKF3_core::FuseMagnetometer() @@ -602,7 +602,7 @@ void NavEKF3_core::FuseMagnetometer()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = SK_MX[0]*(P[stateIndex][19] + P[stateIndex][1]*SH_MAG[0] - P[stateIndex][2]*SH_MAG[1] + P[stateIndex][3]*SH_MAG[2] + P[stateIndex][0]*SK_MX[2] - P[stateIndex][16]*SK_MX[1] + P[stateIndex][17]*SK_MX[4] - P[stateIndex][18]*SK_MX[3]);
} else {
@ -682,7 +682,7 @@ void NavEKF3_core::FuseMagnetometer() @@ -682,7 +682,7 @@ void NavEKF3_core::FuseMagnetometer()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = SK_MY[0]*(P[stateIndex][20] + P[stateIndex][0]*SH_MAG[2] + P[stateIndex][1]*SH_MAG[1] + P[stateIndex][2]*SH_MAG[0] - P[stateIndex][3]*SK_MY[2] - P[stateIndex][17]*SK_MY[1] - P[stateIndex][16]*SK_MY[3] + P[stateIndex][18]*SK_MY[4]);
} else {
@ -764,7 +764,7 @@ void NavEKF3_core::FuseMagnetometer() @@ -764,7 +764,7 @@ void NavEKF3_core::FuseMagnetometer()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = SK_MZ[0]*(P[stateIndex][21] + P[stateIndex][0]*SH_MAG[1] - P[stateIndex][1]*SH_MAG[2] + P[stateIndex][3]*SH_MAG[0] + P[stateIndex][2]*SK_MZ[2] + P[stateIndex][18]*SK_MZ[1] + P[stateIndex][16]*SK_MZ[4] - P[stateIndex][17]*SK_MZ[3]);
} else {
@ -1302,7 +1302,7 @@ void NavEKF3_core::FuseDeclination(float declErr) @@ -1302,7 +1302,7 @@ void NavEKF3_core::FuseDeclination(float declErr)
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = -t4*t13*(P[stateIndex][16]*magE-P[stateIndex][17]*magN);
} else {

4
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
Unescape View File

@ -467,7 +467,7 @@ void NavEKF3_core::FuseOptFlow() @@ -467,7 +467,7 @@ void NavEKF3_core::FuseOptFlow()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = t78*(P[stateIndex][0]*t2*t5-P[stateIndex][4]*t2*t7+P[stateIndex][1]*t2*t15+P[stateIndex][6]*t2*t10+P[stateIndex][2]*t2*t19-P[stateIndex][3]*t2*t22+P[stateIndex][5]*t2*t27);
} else {
@ -643,7 +643,7 @@ void NavEKF3_core::FuseOptFlow() @@ -643,7 +643,7 @@ void NavEKF3_core::FuseOptFlow()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = -t78*(P[stateIndex][0]*t2*t5+P[stateIndex][5]*t2*t8-P[stateIndex][6]*t2*t10+P[stateIndex][1]*t2*t16-P[stateIndex][2]*t2*t19+P[stateIndex][3]*t2*t22+P[stateIndex][4]*t2*t27);
} else {

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Unescape View File

@ -1388,7 +1388,7 @@ void NavEKF3_core::FuseBodyVel() @@ -1388,7 +1388,7 @@ void NavEKF3_core::FuseBodyVel()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = t77*(P[stateIndex][5]*t4+P[stateIndex][4]*t9+P[stateIndex][0]*t14-P[stateIndex][6]*t11+P[stateIndex][1]*t18-P[stateIndex][2]*t21+P[stateIndex][3]*t24);
} else {
@ -1565,7 +1565,7 @@ void NavEKF3_core::FuseBodyVel() @@ -1565,7 +1565,7 @@ void NavEKF3_core::FuseBodyVel()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = t77*(-P[stateIndex][4]*t3+P[stateIndex][5]*t8+P[stateIndex][0]*t15+P[stateIndex][6]*t12+P[stateIndex][1]*t18+P[stateIndex][2]*t22-P[stateIndex][3]*t25);
} else {
@ -1743,7 +1743,7 @@ void NavEKF3_core::FuseBodyVel() @@ -1743,7 +1743,7 @@ void NavEKF3_core::FuseBodyVel()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = t77*(P[stateIndex][4]*t4+P[stateIndex][0]*t14+P[stateIndex][6]*t9-P[stateIndex][5]*t11-P[stateIndex][1]*t17+P[stateIndex][2]*t20+P[stateIndex][3]*t24);
} else {

2
libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
Unescape View File

@ -150,7 +150,7 @@ void NavEKF3_core::FuseRngBcn() @@ -150,7 +150,7 @@ void NavEKF3_core::FuseRngBcn()
if (!inhibitDelVelBiasStates) {
for (uint8_t index = 0; index < 3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (!dvelBiasAxisInhibit[index]) {
Kfusion[stateIndex] = -t26*(P[stateIndex][7]*t4*t9+P[stateIndex][8]*t3*t9+P[stateIndex][9]*t2*t9);
} else {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Unescape View File

@ -1683,7 +1683,7 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr) @@ -1683,7 +1683,7 @@ void NavEKF3_core::CovariancePrediction(Vector3f *rotVarVecPtr)
// interacton with other states
if (!inhibitDelVelBiasStates) {
for (uint8_t index=0; index<3; index++) {
uint8_t stateIndex = index + 13;
const uint8_t stateIndex = index + 13;
if (dvelBiasAxisInhibit[index]) {
zeroCols(nextP,stateIndex,stateIndex);
nextP[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index];

Write Preview
Loading…
Cancel
Save