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