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 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; } }