Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: constify array index variable

This commit is contained in:
Paul Riseborough 2021-03-22 19:47:53 +11:00 committed by Andrew Tridgell
parent 1498b516a3
commit ac87cab6bc
6 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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()
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
View File

@ -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()
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()
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)
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
View File

@ -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()
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
View File

@ -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()
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()
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
View File

@ -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
View File

@ -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];