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 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); }
void UART2_ProcessSpektrumData( ) { // PROCESS Spektrum receiver data -- see http://www.desertrc.com/spektrum_protocol.htm for protocol // Determine what to do with received character uint8_t i; int16_t scaleInt; _Q16 tmpQ16, scaleQ16, scale2Q16; // Data is 14 bytes long for( i=0; i<14; i+=2) { uint16_t rcdata = spektrumData[i] << 8; // MSB rcdata += spektrumData[i+1]; // LSB #ifdef DSMX // 11-bit data mode uint16_t cmddata = (int16_t) (rcdata & 0b0000011111111111); // get last 11 bits uint8_t channel = rcdata >> 11; // get 5 first bits scaleQ16 = num1024; scale2Q16 = num2048; scaleInt = 8; #else // 10-bit data mode uint16_t cmddata = (int16_t) (rcdata & 0b0000001111111111); // get last 10 bits uint8_t channel = rcdata >> 10; // get 6 first bits scaleQ16 = num512; scale2Q16 = num1024; scaleInt = 4; #endif switch(channel){ // process channel data case 0: RCdata.ch0 = cmddata; break; case 1: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch1 = _IQ16div(tmpQ16,scale2Q16); break; case 2: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch2 = _IQ16div(tmpQ16,scale2Q16); break; case 3: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch3 = _IQ16div(tmpQ16,scaleQ16); break; case 4: RCdata.ch4 = cmddata; break; case 5: RCdata.ch5 = cmddata; break; case 6: RCdata.ch6 = cmddata; break; default: break; } // End switch channel } // End for each data byte // manual mode if (RCdata.ch4 > 600) { CmdData.AttCmd = 1; //Debug: Scaling throttle down 2 times //CmdData.throttle = RCdata.ch0/(scaleInt*2); // RCdata is between 0 and 1023, this will be approximately between 0 and 255 //Normal throttle CmdData.throttle = RCdata.ch0/(scaleInt); // RCdata is between 0 and 1023, this will be approximately between 0 and 255 _Q16 halfRoll = -RCdata.ch1; //mult(RCdata.ch1,num0p5); // this is about -0.6 -> 0.6 which is +/- ~70 degrees _Q16 halfPitch = RCdata.ch2; //mult(RCdata.ch2,num0p5); _Q16 cosRoll = _Q16cos(halfRoll); _Q16 sinRoll = _Q16sin(halfRoll); _Q16 cosPitch = _Q16cos(halfPitch); _Q16 sinPitch = _Q16sin(halfPitch); CmdData.q_cmd.o = mult(cosRoll,cosPitch); CmdData.q_cmd.x = mult(sinRoll,cosPitch); CmdData.q_cmd.y = mult(cosRoll,sinPitch); CmdData.q_cmd.z = -mult(sinRoll,sinPitch); CmdData.p = CmdData.q = 0; CmdData.r = mult(RCdata.ch3,num2p0); // Turn on green LED to signify manual control mode ON led_on(LED_GREEN); } else { // Turn off green LED to signify manual control mode OFF led_off(LED_GREEN); CmdData.AttCmd = 0; } }