static int decodepacket( void)
{
if ( rxdata[0] == 0x55 )
{
rx[0] = -cx10scale(9); // aileron
rx[1] = -cx10scale(11) ; // elev
rx[3] = (cx10scale(13) + 1.0f)*0.5f ; // throttle
rx[2] = cx10scale(15) ; // throttle
#ifndef DISABLE_EXPO
rx[0] = rcexpo ( rx[0] , EXPO_XY );
rx[1] = rcexpo ( rx[1] , EXPO_XY );
rx[2] = rcexpo ( rx[2] , EXPO_YAW );
#endif
aux[0] = (rxdata[16] & 0x10)?1:0;
aux[2] = (rxdata[17] & 0x01)?1:0; // rates mid
for ( int i = 0 ; i < AUXNUMBER - 2 ; i++)
{
auxchange[i] = 0;
if ( lastaux[i] != aux[i] ) auxchange[i] = 1;
lastaux[i] = aux[i];
}
return 1; // valid packet
}
return 0; //
}
int decode_cg023( void)
{
if ( rxdata[0] == 0x55 )
{
// maybe corrupt packet
if ( rxdata[3] != 0 || rxdata[4] != 0 ) return 0;
if ( rxdata[1] != txid[0] || rxdata[2] != txid[1] ) return 0;
// throttle
rx[3] = 0.00390625f * rxdata[5];
// swapped yaw - roll (mode 3)
if ( rxdata[6] >= 0x80 )
{
rx[0] = -rxdata[6] * 0.0166666f + 2.1166582f; // yaw
}
else if ( rxdata[6] <= 0x3C ) rx[0] = (1.0f + ( rxdata[6] - 60) * 0.0166666f) ; // yaw
else rx[0] = 0.0;
rx[2] = - rxdata[8] * 0.0166666f + 2.1166582f; // roll
rx[1] = - rxdata[7] * 0.0166666f + 2.1166582f;
#ifndef DISABLE_EXPO
rx[0] = rcexpo ( rx[0] , EXPO_XY );
rx[1] = rcexpo ( rx[1] , EXPO_XY );
rx[2] = rcexpo ( rx[2] , EXPO_YAW );
#endif
// switch flags
aux[0] = (rxdata[13] & CG023_FLIP_MASK)?1:0;
aux[1] = (rxdata[13] & CG023_VIDEO_MASK)?1:0;
aux[2] = (rxdata[13] & CG023_STILL_MASK)?1:0;
aux[3] = (rxdata[13] & CG023_LED_OFF_MASK)?1:0;
float ratemulti = 1.0;
if ( rxdata[13] & CG023_RATE_100_MASK )
{
goto skip;
}
else if ( rxdata[13] & CG023_RATE_60_MASK )
{
ratemulti = CG023_MIDRATE_MULTIPLIER;
}
else
{
ratemulti = CG023_LOWRATE_MULTIPLIER;
}
for ( int i = 0 ; i <= 2 ; i++)
{
rx[i] = rx[i] * ratemulti;
}
skip:
for ( int i = 0 ; i < AUXNUMBER - 2 ; i++)
{
auxchange[i] = 0;
if ( lastaux[i] != aux[i] ) auxchange[i] = 1;
lastaux[i] = aux[i];
}
return 1;
}
else
{
// non data packet
return 0;
}
//
}
static int decodepacket( void)
{
if ( rxdata[0] == 165 )
{
int sum = 0;
for(int i=0; i<14; i++)
{
sum += rxdata[i];
}
if ( (sum&0xFF) == rxdata[14] )
{
rx[0] = packettodata( &rxdata[4] );
rx[1] = packettodata( &rxdata[6] );
rx[2] = packettodata( &rxdata[10] );
// throttle
rx[3] = ( (rxdata[8]&0x0003) * 256 + rxdata[9] ) * 0.000976562f;
#ifndef DISABLE_EXPO
rx[0] = rcexpo ( rx[0] , EXPO_XY );
rx[1] = rcexpo ( rx[1] , EXPO_XY );
rx[2] = rcexpo ( rx[2] , EXPO_YAW );
#endif
// trims are 50% of controls at max
// trims are not used as trims because they interfere
// with dynamic trims feature of devo firmware
// rx[0] = rx[0] + 0.03225 * 0.5 * (float)(((rxdata[4])>>2) - 31);
// rx[1] = rx[1] + 0.03225 * 0.5 * (float)(((rxdata[6])>>2) - 31);
// rx[2] = rx[2] + 0.03225 * 0.5 * (float)(((rxdata[10])>>2) - 31);
// Instead they are used as binary aux channels
#ifdef USE_STOCK_TX
char trims[4];
trims[0] = rxdata[6] >> 2;
trims[1] = rxdata[4] >> 2;
// trims[2] = rxdata[8] >> 2; // throttle and yaw trims are not used
// trims[3] = rxdata[10] >> 2;
for (int i = 0; i < 2; i++)
if (trims[i] != lasttrim[i])
{
aux[CH_PIT_TRIM + i] = trims[i] > lasttrim[i];
lasttrim[i] = trims[i];
}
#else
// this share the same numbers to the above CH_PIT_TRIM etc
aux[CH_VID] = (rxdata[2] & 0x10) ? 1 : 0;
aux[CH_PIC] = (rxdata[2] & 0x20) ? 1 : 0;
#endif
aux[CH_INV] = (rxdata[3] & 0x80)?1:0; // inverted flag
aux[CH_FLIP] = (rxdata[2] & 0x08) ? 1 : 0;
aux[CH_EXPERT] = (rxdata[1] == 0xfa) ? 1 : 0;
aux[CH_HEADFREE] = (rxdata[2] & 0x02) ? 1 : 0;
aux[CH_RTH] = (rxdata[2] & 0x01) ? 1 : 0; // rth channel
for ( int i = 0 ; i < AUXNUMBER - 2 ; i++)
{
auxchange[i] = 0;
if ( lastaux[i] != aux[i] ) auxchange[i] = 1;
lastaux[i] = aux[i];
}
return 1; // valid packet
}
static int decodepacket( void)
{
if ( rxdata[0] == 165 )
{
int sum = 0;
for(int i=0; i<14; i++)
{
sum += rxdata[i];
}
if ( (sum&0xFF) == rxdata[14] )
{
rx[0] = packettodata( &rxdata[4] );
rx[1] = packettodata( &rxdata[6] );
rx[2] = packettodata( &rxdata[10] );
// throttle
rx[3] = ( (rxdata[8]&0x0003) * 256 + rxdata[9] ) * 0.000976562;
#ifndef DISABLE_EXPO
rx[0] = rcexpo ( rx[0] , EXPO_XY );
rx[1] = rcexpo ( rx[1] , EXPO_XY );
rx[2] = rcexpo ( rx[2] , EXPO_YAW );
#endif
if (rxdata[1] != 0xfa)
{// low rates
for ( int i = 0 ; i <3; i++)
{
rx[i] = rx[i] * (float) BAYANG_LOWRATE_MULTIPLIER;
}
}
// trims are 50% of controls at max
// trims are not used because they interfere with dynamic trims feature of devo firmware
// rx[0] = rx[0] + 0.03225 * 0.5 * (float)(((rxdata[4])>>2) - 31);
// rx[1] = rx[1] + 0.03225 * 0.5 * (float)(((rxdata[6])>>2) - 31);
// rx[2] = rx[2] + 0.03225 * 0.5 * (float)(((rxdata[10])>>2) - 31);
// flip channel
aux[CH_FLIP] = (rxdata[2] & 0x08)?1:0;
// expert mode
aux[CH_EXPERT] = (rxdata[1] == 0xfa)?1:0;
// headless channel
aux[CH_HEADFREE] = (rxdata[2] & 0x02)?1:0;
// rth channel
aux[CH_RTH] = (rxdata[2] & 0x01)?1:0;
aux[CH_INV] = (rxdata[1] & 0x80)?1:0; // inverted flag
aux[CH_VID] = (rxdata[2] & 0x10) ? 1 : 0;
aux[CH_HEADFREE] = (rxdata[2] & 0x20) ? 1 : 0;
for ( int i = 0 ; i < AUXNUMBER - 2 ; i++)
{
auxchange[i] = 0;
if ( lastaux[i] != aux[i] ) auxchange[i] = 1;
lastaux[i] = aux[i];
}
return 1; // valid packet
}
return 0; // sum fail
}
return 0; // first byte different
}
