void attEstimatePose(void) {

	// Fixed point implementation
	_Q16 dphi, dtheta, dpsi;
	float rate[3];
	_Q16 wx, wy, wz;
	_Q16 sin_phi, tan_theta, cos_phi, cos_theta, temp;

	gyroGetRadXYZ(rate);  // 50 us

	wx = _Q16ftoi(rate[0]);
	wy = _Q16ftoi(rate[1]);
	wz = _Q16ftoi(rate[2]);

	sin_phi = _Q16sin(PoseQ.qdata.phi);
	cos_phi = _Q16cos(PoseQ.qdata.phi);
	cos_theta = _Q16cos(PoseQ.qdata.theta);
	tan_theta = _Q16tan(PoseQ.qdata.theta);

	temp = _Q16mult(wy, sin_phi) + _Q16mult(wz, cos_phi);
	dphi = _Q16mult(temp, tan_theta) + wx;
	dtheta = _Q16mult(wy, cos_phi) + _Q16neg(_Q16mult(wz, sin_phi));
	dpsi = _IQ16div(temp, cos_theta);

	PoseQ.qdata.phi += _Q16mult(dphi, samplePeriod);
	PoseQ.qdata.theta += _Q16mult(dtheta, samplePeriod);
	PoseQ.qdata.psi += _Q16mult(dpsi, samplePeriod);
}
void attSetup(float ts) {

    samplePeriod = _Q16ftoi(ts);

    // initial values of roll, pitch, and yaw.
    PoseQ.qdata.phi = 0;
    PoseQ.qdata.theta = 0;
    PoseQ.qdata.psi = 0;
}
예제 #3
0
/*---------------------------------------------------------------------
  Function Name: Controller_Update
  Description:   Run the attitude controller, should execute at 1KHz
					Set motor values
  Inputs:        None
  Returns:       None
-----------------------------------------------------------------------*/
void Controller_Update(void)
{
    // Quaternion attitude error
    tQuaternion qerror = qprodconj(AHRSdata.q_est, CmdData.q_cmd);

    // New Quaternion Controller from Frazzoli
    _Q16 pErr = AHRSdata.p - CmdData.p;
    _Q16 qErr = AHRSdata.q - CmdData.q;
    _Q16 rErr = AHRSdata.r - CmdData.r;
    rollCmd = mult(Gains.Kp_roll,qerror.x);
    pitchCmd = mult(Gains.Kp_pitch,qerror.y);
    yawCmd = mult(Gains.Kp_yaw,qerror.z);
    if (qerror.o < 0){
        rollCmd = -rollCmd;
        pitchCmd = -pitchCmd;
        yawCmd = -yawCmd;
    }

    // Increment Integrators if motor commands are being sent
    if (1 == CmdData.AttCmd || 2 == CmdData.AttCmd) {
        if (Gains.IntRoll < _Q16ftoi(-1.0)){
            Gains.IntRoll = _Q16ftoi(-0.99);
        } else if (Gains.IntRoll > _Q16ftoi(1.0)) {
            Gains.IntRoll = _Q16ftoi(0.99);
        } else {
            Gains.IntRoll += mult(rollCmd,Gains.dt);
        }

        if (Gains.IntPitch < _Q16ftoi(-1.0)){
            Gains.IntPitch = _Q16ftoi(-0.99);
        } else if (Gains.IntPitch > _Q16ftoi(1.0)) {
            Gains.IntPitch = _Q16ftoi(0.99);
        } else {
            Gains.IntPitch += mult(pitchCmd,Gains.dt);
        }

        if (Gains.IntYaw < _Q16ftoi(-1.0)){
            Gains.IntYaw = _Q16ftoi(-0.99);
        } else if (Gains.IntYaw > _Q16ftoi(1.0)) {
            Gains.IntYaw = _Q16ftoi(0.99);
        } else {
            Gains.IntYaw += mult(yawCmd,Gains.dt);
        }
    } else {
        Gains.IntRoll = _Q16ftoi(0.0);
        Gains.IntPitch = _Q16ftoi(0.0);
        Gains.IntYaw = _Q16ftoi(0.0);
    }


    rollCmd += mult(Gains.Ki_roll,Gains.IntRoll) - mult(Gains.Kd_roll,pErr);
    pitchCmd += mult(Gains.Ki_pitch,Gains.IntPitch) - mult(Gains.Kd_pitch,qErr);
    yawCmd += mult(Gains.Ki_yaw,Gains.IntYaw) - mult(Gains.Kd_yaw,rErr);
    // End New Quaternion controller

    // Form motor commands
    _Q16 m1 = -pitchCmd - yawCmd;
    _Q16 m2 = -rollCmd  + yawCmd;
    _Q16 m3 =  pitchCmd - yawCmd;
    _Q16 m4 =  rollCmd  + yawCmd;

    // DEBUG
    //CmdData.throttle = 0;
    //CmdData.AttCmd = 1;
    // END DEBUG

    // Get throttle value
    int16_t tmp = (int16_t) CmdData.throttle;
    _Q16 throttle = 0;
    int16toQ16(&throttle,&tmp);     // Throttle between 0.0 and 255.0 here

    // DEBUG
    //m1 = 0; m2 = 0; m3 = 0; m4 = 0;

    if (1 == CmdData.AttCmd || 2 == CmdData.AttCmd) {

        /**** PWM motors - take value between 0 and 3685 (converted to 1 <--> 2 ms PWM signal at 490Hz)*/
        int16_t pwmData = 0;
        throttle = mult(throttle,num14p45);  // multiply throttle by 14.45 to get 0 to 3685  ( assuming throttle is between 0 and 255)
        _Q16 motorCmd = m1 + throttle;
        _Q16sat(&motorCmd, num3685, 0);     // TODO: find lower saturation bound here so motors don't turn off
        Q16toint16(&pwmData,&motorCmd);
        SETPWM(PWM1,pwmData);

        motorCmd = m2 + throttle;
        _Q16sat(&motorCmd, num3685, 0);     // TODO: find lower saturation bound here so motors don't turn off
        Q16toint16(&pwmData,&motorCmd);
        SETPWM(PWM2,pwmData);

        motorCmd = m3 + throttle;
        _Q16sat(&motorCmd, num3685, 0);     // TODO: find lower saturation bound here so motors don't turn off
        Q16toint16(&pwmData,&motorCmd);
        SETPWM(PWM3,pwmData);

        motorCmd = m4 + throttle;
        _Q16sat(&motorCmd, num3685, 0);     // TODO: find lower saturation bound here so motors don't turn off
        Q16toint16(&pwmData,&motorCmd);
        SETPWM(PWM4,pwmData);

 

    } else if (0 == CmdData.AttCmd) {

      	// set motor values to off
        SETPWM(PWM1,0);
        SETPWM(PWM2,0);
        SETPWM(PWM3,0);
        SETPWM(PWM4,0);

    }

}
예제 #4
0
/*---------------------------------------------------------------------
  Function Name: Controller_Init
  Description:   Initialize all the controller variables
  Inputs:        None
  Returns:       None
-----------------------------------------------------------------------*/
void Controller_Init(void)
{
    // ESCs
    SETPWM(PWM1,0);
    SETPWM(PWM2,0);
    SETPWM(PWM3,0);
    SETPWM(PWM4,0);

    CmdData.q_cmd.o = _Q16ftoi(1.0);
    CmdData.q_cmd.x = _Q16ftoi(0.0);
    CmdData.q_cmd.y = _Q16ftoi(0.0);
    CmdData.q_cmd.z = _Q16ftoi(0.0);
    CmdData.p = _Q16ftoi(0.0);
    CmdData.q = _Q16ftoi(0.0);
    CmdData.r = _Q16ftoi(0.0);
    CmdData.throttle = 0;
    CmdData.AttCmd = 0;

    // mQxx default values
    // Working gains
    /*
    Gains.Kp_roll = _Q16ftoi(354.0);
    Gains.Ki_roll = _Q16ftoi(0.0);
    Gains.Kd_roll = _Q16ftoi(118.0);

    Gains.Kp_pitch = _Q16ftoi(354.0);
    Gains.Ki_pitch = _Q16ftoi(0.0);
    Gains.Kd_pitch = _Q16ftoi(118.0);

    Gains.Kp_yaw = _Q16ftoi(0.0);
    Gains.Ki_yaw = _Q16ftoi(0.0);
    Gains.Kd_yaw = _Q16ftoi(184.0);

    Gains.maxang = _Q16ftoi(0.8);

    Gains.IntRoll = _Q16ftoi(0.0);
    Gains.IntPitch = _Q16ftoi(0.0);
    Gains.IntYaw = _Q16ftoi(0.0);
    Gains.dt = _Q16ftoi(1.0/1000.0);

     */
    Gains.Kp_roll = _Q16ftoi(2000.0);
    Gains.Ki_roll = _Q16ftoi(0.0);
    Gains.Kd_roll = _Q16ftoi(400.0);

    Gains.Kp_pitch = _Q16ftoi(2000.0);
    Gains.Ki_pitch = _Q16ftoi(0.0);
    Gains.Kd_pitch = _Q16ftoi(400.0);

    Gains.Kp_yaw = _Q16ftoi(0.0);
    Gains.Ki_yaw = _Q16ftoi(0.0);
    Gains.Kd_yaw = _Q16ftoi(184.0);

    Gains.maxang = _Q16ftoi(0.8);

    Gains.IntRoll = _Q16ftoi(0.0);
    Gains.IntPitch = _Q16ftoi(0.0);
    Gains.IntYaw = _Q16ftoi(0.0);
    Gains.dt = _Q16ftoi(1.0/2000.0);
    
}
예제 #5
0
void AHRS_GyroProp(void){
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Read raw gyro values from A2D registers, A2D automatically scans inputs at 5KHz
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    SensorData.gyroX = xgyro; SensorData.gyroX -= SensorCal.gyroRawBias;
    SensorData.gyroY = ygyro; SensorData.gyroY -= SensorCal.gyroRawBias;
    SensorData.gyroZ = zgyro; SensorData.gyroZ -= SensorCal.gyroRawBias;

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Gyro scaling, to rad/sec
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    int16toQ16(&AHRSdata.p, &SensorData.gyroX);
    AHRSdata.p = -mult( AHRSdata.p, SensorCal.gyroScale);
    int16toQ16(&AHRSdata.q, &SensorData.gyroY);
    AHRSdata.q = mult( AHRSdata.q, SensorCal.gyroScale );
    int16toQ16(&AHRSdata.r, &SensorData.gyroZ);
    AHRSdata.r = mult( AHRSdata.r, SensorCal.gyroScale );

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Initial gyro bias calculation
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    if( SensorCal.biasCountGyro < SensorCal.biasTotalGyro){
        // Do some blank reads to clear any garbage in the initial transient
        if(--SensorCal.blankReadsGyro > 0)
            return;

        SensorCal.pBias += AHRSdata.p;
        SensorCal.qBias += AHRSdata.q;
        SensorCal.rBias += AHRSdata.r;

        led_on(LED_RED);

        if( ++SensorCal.biasCountGyro == SensorCal.biasTotalGyro ){
            _Q16 tmp = _Q16ftoi(1.0 / ((float)SensorCal.biasTotalGyro  ));
            SensorCal.pBias = mult( SensorCal.pBias, tmp);
            SensorCal.qBias = mult( SensorCal.qBias, tmp);
            SensorCal.rBias = mult( SensorCal.rBias, tmp);
            led_off(LED_RED);
            led_off(LED_GREEN);
        }


        // TODO: Initialize q_est to q_meas


        return;
    }

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Gyro bias correction
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    AHRSdata.p -= SensorCal.pBias;
    AHRSdata.q -= SensorCal.qBias;
    AHRSdata.r -= SensorCal.rBias;

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Gyro propagation
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    
    AHRSdata.q_est.o -=  mult(  mult(AHRSdata.q_est.x,AHRSdata.p) +  mult(AHRSdata.q_est.y,AHRSdata.q) +  mult(AHRSdata.q_est.z,AHRSdata.r) , SensorCal.gyroPropDT);
    AHRSdata.q_est.x +=  mult(  mult(AHRSdata.q_est.o,AHRSdata.p) -  mult(AHRSdata.q_est.z,AHRSdata.q) +  mult(AHRSdata.q_est.y,AHRSdata.r) , SensorCal.gyroPropDT);
    AHRSdata.q_est.y +=  mult(  mult(AHRSdata.q_est.z,AHRSdata.p) +  mult(AHRSdata.q_est.o,AHRSdata.q) -  mult(AHRSdata.q_est.x,AHRSdata.r) , SensorCal.gyroPropDT);
    AHRSdata.q_est.z +=  mult(  mult(AHRSdata.q_est.x,AHRSdata.q) -  mult(AHRSdata.q_est.y,AHRSdata.p) +  mult(AHRSdata.q_est.o,AHRSdata.r) , SensorCal.gyroPropDT);
    
    // Run the attitude control after propogating gyros
    loop.AttCtl = 1;
}
예제 #6
0
// Initialize
void AHRS_init(void){

    // Setup calibration struct
    SensorCal.biasCountGyro = 0;
    SensorCal.biasTotalGyro = 2000;
    SensorCal.blankReadsGyro = 200;
    SensorCal.biasCountAcc = 0;
    SensorCal.biasTotalAcc = 2000;
    SensorCal.blankReadsAcc = 200;
    SensorCal.pBias = _Q16ftoi(0.0);
    SensorCal.qBias = _Q16ftoi(0.0);
    SensorCal.rBias = _Q16ftoi(0.0);

    // Initialize attitude
    AHRSdata.q_est.o = _Q16ftoi(1.0);
    AHRSdata.q_est.x = _Q16ftoi(0.0);
    AHRSdata.q_est.y = _Q16ftoi(0.0);
    AHRSdata.q_est.z = _Q16ftoi(0.0);
    AHRSdata.q_meas.o = _Q16ftoi(1.0);
    AHRSdata.q_meas.x = _Q16ftoi(0.0);
    AHRSdata.q_meas.y = _Q16ftoi(0.0);
    AHRSdata.q_meas.z = _Q16ftoi(0.0);

    // Parameters
    SensorCal.gyroRawBias = 512; // Mid-range value for 10-bit A2D
    SensorCal.accelRawBias= 500; // Mid-range value for 10-bit A2D (adjusted manually)
    SensorCal.gyroScale = _Q16ftoi(PI/180.0 / 1024.0 * 3.3 / 0.0033  * 1.5 ); // Based on 10-bit A2D and gyro sensitivity of 3.3mV / deg/s with fudge factor
    SensorCal.gyroPropDT = _Q16ftoi(0.001 / 2.0);

    SensorCal.accelScale = _Q16ftoi( 3.3 / 1024.0 / 0.200 ); // at 6g resolution, sensitivity is 200mV/g
    SensorCal.acc_window_min = _Q16ftoi(0.5);
    SensorCal.acc_window_max = _Q16ftoi(1.5);
    SensorCal.axBias = _Q16ftoi(0.0);
    SensorCal.ayBias = _Q16ftoi(0.0);
    SensorCal.azBias = _Q16ftoi(0.0);

    SensorCal.K_AttFilter = _Q16ftoi(0.025); // Default value, may be overwritten by SensorPacket
}
예제 #7
0
void AHRS_AccMagCorrect(void)
{
    // Quit if the biases are still being calculated
    if( SensorCal.biasCountGyro < SensorCal.biasTotalGyro)
        return;

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Read raw accel values from A2D registers, A2D automatically scans inputs at 5KHz
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    SensorData.accX = xaccel; SensorData.accX -= SensorCal.accelRawBias;
    SensorData.accY = yaccel; SensorData.accY -= SensorCal.accelRawBias;
    SensorData.accZ = zaccel; SensorData.accZ -= SensorCal.accelRawBias;
    

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Accel scaling, to g
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    int16toQ16(&AHRSdata.ax, &SensorData.accX);
    AHRSdata.ax = -mult( AHRSdata.ax, SensorCal.accelScale);
    int16toQ16(&AHRSdata.ay, &SensorData.accY);
    AHRSdata.ay = mult( AHRSdata.ay, SensorCal.accelScale );
    int16toQ16(&AHRSdata.az, &SensorData.accZ);
    AHRSdata.az = -mult( AHRSdata.az, SensorCal.accelScale );


    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Initial acc bias calculation
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    if( SensorCal.biasCountAcc < SensorCal.biasTotalAcc){
        // Do some blank reads to clear any garbage in the initial transient
        if(--SensorCal.blankReadsAcc > 0)
            return;

        SensorCal.axBias += AHRSdata.ax;
        SensorCal.ayBias += AHRSdata.ay;
        SensorCal.azBias += AHRSdata.az;

        led_on(LED_RED);

        if( ++SensorCal.biasCountAcc == SensorCal.biasTotalAcc ){
            _Q16 tmp = _Q16ftoi(1.0 / ((float)SensorCal.biasTotalAcc  ));
            SensorCal.axBias = mult( SensorCal.axBias, tmp);
            SensorCal.ayBias = mult( SensorCal.ayBias, tmp);
            SensorCal.azBias = mult( SensorCal.azBias, tmp);
            led_off(LED_RED);
            led_off(LED_GREEN);
        }


        return;
    }

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Acc bias correction
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$

    AHRSdata.ax = AHRSdata.ax - SensorCal.axBias;
    AHRSdata.ay = AHRSdata.ay - SensorCal.ayBias;
    AHRSdata.az = AHRSdata.az - SensorCal.azBias;

    _Q16 ax = AHRSdata.ax;
    _Q16 ay = AHRSdata.ay;
    _Q16 az = AHRSdata.az;

    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // Maneuver detector, do not use accels during fast movement
    // $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
    // TODO: Check angular rates

    // Roll and pitch calculation, assumes accelerometer units are 10000*g
    // Normalize the acceleration vector to length 1

    _Q16 root =  _Q16sqrt( mult(ax,ax) + mult(ay,ay) + mult(az,az));

    // TODO: Make sure we're around 1g
    if( root < SensorCal.acc_window_min || root > SensorCal.acc_window_max){
        return;
    }

    // Normalize
    ax = _IQ16div(ax, root);
    ay = _IQ16div(ay, root);
    az = _IQ16div(az, root);

    // Too close to singularity (due to numerical precision limits)
    if( ax > num0p998 || -ax > num0p998 )
        return;

    root = _Q16sqrt( mult(ay,ay) + mult(az,az));
    if(root < num0p0001 )
        root = num0p0001;

    // Calculate sin/cos of roll and pitch
    _Q16 sinR = - _IQ16div(ay,root);
    _Q16 cosR = - _IQ16div(az,root);

    _Q16 sinP = ax;
    _Q16 cosP = -( mult(ay,sinR) + mult(az,cosR) );

    // Calculate half-angles
    _Q16 cosR2 = _Q16sqrt( mult( num1p0 + cosR , num0p5  ));
    if(cosR2 < num0p0001 )
        cosR2 = num0p0001;

    _Q16 sinR2 = mult(_IQ16div( sinR , cosR2) ,  num0p5 ); // WARNING: This step is numerically ill-behaved!

    _Q16 cosP2 = _Q16sqrt( mult( num1p0 + cosP , num0p5  ));
    if(cosP2 < num0p0001 )
        cosP2 = num0p0001;

    _Q16 sinP2 = mult(_IQ16div( sinP , cosP2) ,  num0p5 ); // WARNING: This step is numerically ill-behaved!

    // Too close to singularity (due to numerical precision limits)
    if( mult(cosR2,cosR2) + mult(sinR2,sinR2) > num1p1 || mult(cosP2,cosP2) + mult(sinP2,sinP2) > num1p1 )
        return;

    // Yaw calculation
    // Normalize the magnetometer vector to length 1
/*	magx = (float)AHRSdata.magY;
    magy = -(float)AHRSdata.magX;
    magz = (float)AHRSdata.magZ;
    // Todo: magx*magx can be done in fixed pt
    root = sqrt( magx*magx + magy*magy + magz*magz );
    magx /= root;
    magy /= root;
    magz /= root;
    yaw = atan2(-cosR*magy - sinR*magz  ,  cosP*magx+sinP*sinR*magy-sinP*cosR*magz);
    yaw += PI;
    if(yaw > PI){
            yaw -= 2*PI;
    }
    sinY2 = sin(yaw/2.0);
    cosY2 = cos(yaw/2.0);
    */
    _Q16 cosY2 = _Q16ftoi(1.0);
    _Q16 sinY2 = 0;

    // Finally get quaternion
    tQuaternion qroll,qpitch,qyaw;
    qyaw.o   = cosY2; qyaw.x = 0;      qyaw.y = 0;       qyaw.z = sinY2;
    qpitch.o = cosP2; qpitch.x = 0;    qpitch.y = sinP2; qpitch.z = 0;
    qroll.o  = cosR2; qroll.x = sinR2; qroll.y = 0;      qroll.z = 0;

    AHRSdata.q_meas = qprod(qyaw,qpitch);
    AHRSdata.q_meas = qprod(AHRSdata.q_meas, qroll);

    // Check if flipped from last measurement
    if( mult(AHRSdata.q_meas.x,AHRSdata.q_est.x) + mult(AHRSdata.q_meas.y,AHRSdata.q_est.y) + mult(AHRSdata.q_meas.z,AHRSdata.q_est.z) + mult(AHRSdata.q_meas.o,AHRSdata.q_est.o) < 0 )
    {
        AHRSdata.q_meas.o = - AHRSdata.q_meas.o;
        AHRSdata.q_meas.x = - AHRSdata.q_meas.x;
        AHRSdata.q_meas.y = - AHRSdata.q_meas.y;
        AHRSdata.q_meas.z = - AHRSdata.q_meas.z;
    }

    // Gyro bias estimation

    // Make the correction
    AHRSdata.q_est.o -= mult(AHRSdata.q_est.o-AHRSdata.q_meas.o, SensorCal.K_AttFilter);
    AHRSdata.q_est.x -= mult(AHRSdata.q_est.x-AHRSdata.q_meas.x, SensorCal.K_AttFilter);
    AHRSdata.q_est.y -= mult(AHRSdata.q_est.y-AHRSdata.q_meas.y, SensorCal.K_AttFilter);
    AHRSdata.q_est.z -= mult(AHRSdata.q_est.z-AHRSdata.q_meas.z, SensorCal.K_AttFilter);

}