When I add two numbers together, nothing happens. This is the test code:
Vector3D OrientationBoard::GetGravityVector()
{
// returns gravity vector derived from accelerations
Vector3D ReturnVector;
double dblAxis1Grav;
double dblAxis2Grav;
double dblAxis3Grav;
double dblWork; // work variable
double dblWork2; // work variable
Serial.println("in GetGravityVector");
// gravity is the average of all the accelerations (more or less)
dblAxis1Grav = 0.0;
dblAxis2Grav = 0.0;
dblAxis3Grav = 0.0;
for (int intLoop = 0; intLoop < intDesiredHistoryDepth; intLoop++)
{
dblWork = (double)SensorStream[intLoop].Accel01;
// dblAxis1Grav += (double)SensorStream[intLoop].Accel01;
// dblAxis1Grav += dblWork; <==== doesn't work
// dblAxis1Grav = dblAxis1Grav + dblWork; <==== doesn't work
// dblAxis1Grav = dblWork; // this works, but it's not what I need
dblWork2 = dblAxis1Grav;
dblAxis1Grav = dblWork + dblWork2; // <=== doesn't work
dblAxis2Grav += (double)SensorStream[intLoop].Accel02;
// dblAxis2Grav = (float)7.0; <=== works
dblAxis3Grav += (double)SensorStream[intLoop].Accel03;
// flJunk = SensorStream[intLoop].Accel03;
// dblAxis3Grav = dblAxis3Grav + flJunk; // <=== doesn't work
Serial.println();
Serial.print(" intLoop: ");
Serial.print(intLoop);
// Serial.print(" flJunk: ");
// Serial.print(flJunk);
Serial.print(" SensorStream[intLoop].Accel01: ");
Serial.print(SensorStream[intLoop].Accel01);
Serial.print(" SensorStream[intLoop].Accel02: ");
Serial.print(SensorStream[intLoop].Accel02);
Serial.print(" SensorStream[intLoop].Accel03: ");
Serial.print(SensorStream[intLoop].Accel03);
Serial.print(" dblWork: ");
Serial.print(dblWork);
Serial.print(" dblAxis1Grav: ");
Serial.print(dblAxis1Grav);
Serial.print(" dblAxis2Grav: ");
Serial.print(dblAxis2Grav);
Serial.print(" dblAxis3Grav: ");
Serial.println(dblAxis3Grav);
}
dblAxis1Grav = dblAxis1Grav/(double)intDesiredHistoryDepth;
dblAxis2Grav = dblAxis2Grav/(double)intDesiredHistoryDepth;
dblAxis3Grav = dblAxis3Grav/(double)intDesiredHistoryDepth;
ReturnVector.Axis01 = dblAxis1Grav;
ReturnVector.Axis02 = dblAxis2Grav;
ReturnVector.Axis03 = dblAxis3Grav;
return ReturnVector;
}
Result:
in GetGravityVector
intLoop: 0 SensorStream[intLoop].Accel01: 0.00 SensorStream[intLoop].Accel02: 0.00 SensorStream[intLoop].Accel03: 0.00 dblWork: 0.00 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 1 SensorStream[intLoop].Accel01: 0.00 SensorStream[intLoop].Accel02: 0.00 SensorStream[intLoop].Accel03: 0.00 dblWork: 0.00 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 2 SensorStream[intLoop].Accel01: 0.00 SensorStream[intLoop].Accel02: 0.00 SensorStream[intLoop].Accel03: 2048.00 dblWork: 0.00 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 3 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: 0.00 SensorStream[intLoop].Accel03: 0.00 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 4 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.77 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 5 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.77 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 6 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.77 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 7 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.92 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 8 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.77 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
intLoop: 9 SensorStream[intLoop].Accel01: 0.92 SensorStream[intLoop].Accel02: -0.92 SensorStream[intLoop].Accel03: -9.50 dblWork: 0.92 dblAxis1Grav: 0.00 dblAxis2Grav: 0.00 dblAxis3Grav: 0.00
***************** returned gravity vectors ******************
From roll board: (0.00, 0.00, 0.00)
From pitch board: (0.00, 0.00, 0.00)
Average of two vectors: (0.00, 0.00, 0.00)
***************** returned gravity vectors ****************
Same problem with this code:
double OrientationBoard::CalcAccVal( int intValueDesired) // returns a smoothed acceleration value
{
// returns a smoothed acceleration value
// intValue Desired acceleration value
double AccVal=0.0; // the returned accleration value
double sum;
Serial.println("Enter CalcAccVal");
Serial.print("intValueDesired: ");
Serial.print(intValueDesired);
Serial.println();
Serial.println("Weighted average loop");
Serial.println();
for (int intLoop = 0; intLoop < intDesiredHistoryDepth; intLoop++)
{
Serial.println();
Serial.println("Weights[intLoop]: ");
Serial.println(Weights[intLoop]);
Serial.println();
switch (intValueDesired)
{
case 1: // acceleration 1
AccVal+= SensorStream[intLoop].Accel01Weights[intLoop];
break;
case 2: // acceleration 2
AccVal+= SensorStream[intLoop].Accel02Weights[intLoop];
break;
case 3: // acceleration 3
AccVal+= SensorStream[intLoop].Accel03Weights[intLoop];
break;
case 4: // rotation
AccVal+= SensorStream[intLoop].YawRate01Weights[intLoop];
}
Serial.print("Weighted average loop: ");
Serial.println(AccVal);
}
// return the average
Serial.println();
Serial.print("Returned weighted average: ");
Serial.println(AccVal);
Serial.println("Leaving CalcACCVal");
Serial.println();
return AccVal;
}
Result:
Enter CalcAccVal
intValueDesired: 1
Weighted average loop
Weights[intLoop]:
0.22
Weighted average loop: 0.00
Weights[intLoop]:
0.18
Weighted average loop: 0.00
Weights[intLoop]:
0.14
Weighted average loop: 0.00
Weights[intLoop]:
0.11
Weighted average loop: 0.00
Weights[intLoop]:
0.09
Weighted average loop: 0.00
Weights[intLoop]:
0.07
Weighted average loop: 0.00
Weights[intLoop]:
0.06
Weighted average loop: 0.00
Weights[intLoop]:
0.05
Weighted average loop: 0.00
Weights[intLoop]:
0.04
Weighted average loop: 0.00
Weights[intLoop]:
0.03
Weighted average loop: 0.00
Returned weighted average: 0.00
Leaving CalcACCVal
Thanks for any help or ideas!