void MadgwickAHRS::update(const float *gyro, const float *acc, const float *mag,
Quaternion<float> *Q, const float dT){
chDbgCheck(true == ready, "AHRS not ready");
Vector3d<float> Gyroscope(gyro);
Vector3d<float> Accelerometer(acc);
Vector3d<float> Magnetometer(mag);
Quaternion<float> step(1,0,0,0);
Accelerometer.normalize();
Magnetometer.normalize();
Quaternion<float> tmp;
Quaternion<float> qDot;
Quaternion<float> qcon(1,0,0,0); /* complex conjunction to main quaternion */
//h = quatmult(q, quatmult([0 Magnetometer].', quatcon(q)));
Quaternion<float> qmag(0, Magnetometer(0), Magnetometer(1), Magnetometer(2));
Q->ccon(&qcon);
qmag.mul(&qcon, &tmp);
Q->mul(&tmp, &h);
//b = [norm([m_n m_e]) m_u 0];
//% Reference direction of Earth's G feild
//d = [0 1 0];
b(0) = sqrtf(h(1)*h(1) + h(3)*h(3));
b(1) = h(2);
b(2) = 0;
//Gradient decent algorithm corrective step
F_m(Q, &d, &Accelerometer, &Ftop);
F_m(Q, &b, &Magnetometer, &Fdown);
J_m(Q, &d, &Jtop);
J_m(Q, &b, &Jdown);
J.transpose(&JT);
JT.mul(&F, &step);
step.normalize();
Gyroscope -= &Wb;
//Compute rate of change of quaternion
//qDot_w = 0.5*quatmult(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)].');
//qDot = qDot_w - obj.Beta * step;
//obj.Q_w = q_w/norm(q_w);
tmp(0) = 0;
tmp(1) = gyro[0];
tmp(2) = gyro[1];
tmp(3) = gyro[2];
Q->mul(&tmp, &qDot);
qDot *= 0.5f;
step *= *beta;
qDot -= &step;
//Integrate to yield quaternion
//q = q + qDot*obj.dT;
//obj.Q = q/norm(q); % normalise quaternion
qDot *= dT;
*Q += &qDot;
Q->normalize();
//delta_wb = 2*obj.Zeta*obj.dT * quatmult(quatcon(q), step);
Q->ccon(&qcon);
qcon.mul(&step, &tmp);
tmp *= *zeta * 2 * dT;
//Wb = wb+delta_wb(2:4);
Wb(0) = tmp(1);
Wb(1) = tmp(2);
Wb(2) = tmp(3);
}
#include "atmicro.h"
#include "config.h"
#include "box/interface.h"
ParallelTextLCD lcd = ParallelTextLCD(IOPORTB, IOPIND4, IOPIND3, IOPIND2);
DigitalOutput backlight = DigitalOutput(IOPIND5);
DigitalOutput buzzer = DigitalOutput(IOPIND6);
DigitalOutput led = DigitalOutput(IOPIND7);
Accelerometer acclm = Accelerometer(ADC0, ADC1, ADC2);
Keypad pad = Keypad(IOPORTC);
State state;
Istream istream;
int main(void)
{
acclm.start();
delay(1000);
backlight.on();
lcd.clear();
state.setLockAccelerations(acclm);
delay(1000);
while(1)
{
istream.update(pad);
interface(istream, lcd, state, acclm);
#if DEBUG_MODE == true
lcd.print(0, 1, acclm.x - state.lockAccX);
lcd.print(4, 1, acclm.y - state.lockAccY);
lcd.print(8, 1, acclm.z - state.lockAccZ);
lcd.print(13, 1, state.safe);
#endif
