Mat matrixAllocUnit(int dim) {
Mat m;
m = matrixAlloc(dim, dim);
matrixUnit(m);
return m;
}
//
// updateRotationMatrix - read the accel/mag/gyro, calculate the change in the angle, and update the rotation matrix
//
void Sensors::updateRotationMatrix(float* gyroValues, float* accelValues, float* magValues) {
//
// calculate angular change from gyros
//
// figure out how long it's been since our last update
unsigned long curTime = micros();
float timeDelta = curTime - lastUpdateTime;
lastUpdateTime = curTime;
// calculate angular change from gyro values
for (int i = 0; i < 3; i++) {
gyroValues[i] *= timeDelta / 1000000.0f * DEG2RAD;
}
//
// calculate angular change from each of mag and accel
//
// convert mag and accel to unit vectors
matrixUnit(magValues);
matrixUnit(accelValues);
// invert accel so gravity is correctly pointing down
for (int i = 0; i < 3; i++) {
accelValues[i] = -1.0f * accelValues[i];
}
// adjust mag to account for downward component of earth's magnetic field
float scalar = matrixDot(magValues, accelValues);
for (int i = 0; i < 3; i++) {
magValues[i] -= scalar * accelValues[i];
}
matrixUnit(magValues);
// calculate angular change from mag and accel
for (int i = 0; i < 3; i++) {
magValues[i] = magValues[i] - rotation[0][i];
accelValues[i] = accelValues[i] - rotation[2][i];
}
float magAngle[3];
matrixCross(rotation[0], magValues, magAngle);
float accelAngle[3];
matrixCross(rotation[2], accelValues, accelAngle);
//
// weight gyros, mag, and accel to calculate final answer
//
float finalAngle[3];
for (int i = 0; i < 3; i++) {
finalAngle[i] = gyroValues[i] * GYRO_WEIGHT + magAngle[i] * MAG_WEIGHT + accelAngle[i] * ACCEL_WEIGHT;
}
//
// update rotation matrix with new values
//
// calculate new vectors
float I[3], K[3];
matrixCross(finalAngle, rotation[0], I);
matrixCross(finalAngle, rotation[2], K);
// update the rotation matrix
for (int i = 0; i < 3; i++) {
rotation[0][i] += I[i];
rotation[2][i] += K[i];
}
//
// check that the rotation matrix vectors are still perpendicular and unit length
//
float error = matrixDot(rotation[0], rotation[2]) / 2;
float temp[3] = {rotation[0][0], rotation[0][1], rotation[0][2]};
for (int i = 0; i < 3; i++) {
rotation[0][i] = rotation[0][i] - error * rotation[2][i];
rotation[2][i] = rotation[2][i] - error * temp[i];
}
matrixUnit(rotation[0]);
matrixUnit(rotation[2]);
//
// calculate the J vector and fill in the last row of the rotation matrix
//
matrixCross(rotation[2], rotation[0], rotation[1]);
}
//
// init
//
void Sensors::init(GPS* gpsPointer, Mag *magPointer, Accel* accelPointer, Gyro* gyroPointer, Barometer* barometerPointer, SingleWire* singleWirePointer) {
gps = gpsPointer;
mag = magPointer;
accel = accelPointer;
gyro = gyroPointer;
barometer = barometerPointer;
singleWire = singleWirePointer;
gps->init();
accel->init();
mag->init();
gyro->init();
barometer->init();
singleWire->init();
// calibrate any sensors that need calibration
for (int i = 0; i < CALIBRATION_ROUNDS; i++) {
delay(1000);
mag->calibrate(i);
accel->calibrate(i);
gyro->calibrate(i);
singleWire->calibrate(i);
}
// init variables
lastUpdateTime = micros();
// generate initial rotation matrix
float gyroValues[3];
float accelValues[3];
float magValues[3];
boolean gotValues = false;
do {
delay(200);
if (mag->readRawValues(magValues) && accel->readRawValues(accelValues) && gyro->readRawValues(gyroValues)) {
for (int i = 0; i++; i < 3) {
sensorData.gyro_b[i] = gyroValues[i];
}
// convert accels and mag to unit vectors
matrixUnit(accelValues);
matrixUnit(magValues);
// we want the positive K vector, so invert gravity
accelValues[0] = -accelValues[0];
accelValues[1] = -accelValues[1];
accelValues[2] = -accelValues[2];
// adjust mag to account for downward component of earth's magnetic field
float scalar = matrixDot(magValues, accelValues);
for (int i = 0; i < 3; i++) {
magValues[i] -= scalar * accelValues[i];
}
matrixUnit(magValues);
// calculate K x I = J
float j[3];
matrixCross(accelValues, magValues, j);
rotation[0][0] = magValues[0];
rotation[0][1] = magValues[1];
rotation[0][2] = magValues[2];
rotation[1][0] = j[0];
rotation[1][1] = j[1];
rotation[1][2] = j[2];
rotation[2][0] = accelValues[0];
rotation[2][1] = accelValues[1];
rotation[2][2] = accelValues[2];
gotValues = true;
}
} while (gotValues == false);
}
