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