ROBOFRIEN_SBUS.cpp

00001 #include "ROBOFRIEN_SBUS.h"
00002 #include "millis.h"
00003 #include "BufferedSerial.h"
00004 
00005 BufferedSerial SBUS_(p13,p14);
00006 
00007 #define CAP_MIN     487
00008 #define CAP_NEU     1024
00009 #define CAP_MAX     1561
00010 bool reverse_bool[16] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
00011 
00012 void ROBOFRIEN_SBUS::Init(){
00013     SBUS_.baud(100000);
00014     SBUS_.format(8, Serial::Even, 2);
00015     CONTROLLER_STATE = SIGNAL_OFF;
00016 }
00017 
00018 uint8_t BIT_REVERSE(uint8_t INPUT){
00019     uint8_t OUTPUT;
00020     OUTPUT = INPUT << 7;
00021     OUTPUT |= ( ((INPUT >> 1)&0b00000001) << 6 ); 
00022     OUTPUT |= ( ((INPUT >> 2)&0b00000001) << 5 ); 
00023     OUTPUT |= ( ((INPUT >> 3)&0b00000001) << 4 ); 
00024     OUTPUT |= ( ((INPUT >> 4)&0b00000001) << 3 ); 
00025     OUTPUT |= ( ((INPUT >> 5)&0b00000001) << 2 ); 
00026     OUTPUT |= ( ((INPUT >> 6)&0b00000001) << 1 ); 
00027     OUTPUT |= ( ((INPUT >> 7)&0b00000001) << 0 ); 
00028     return OUTPUT;
00029 }
00030 uint16_t BIT11_REVERSE(uint16_t INPUT){
00031     uint16_t OUTPUT;
00032     INPUT = INPUT & 0b0000011111111111;
00033     OUTPUT = INPUT << 10;
00034     OUTPUT |= ( ((INPUT >> 1)&0b0000000000000001) << 9 ); 
00035     OUTPUT |= ( ((INPUT >> 2)&0b0000000000000001) << 8 ); 
00036     OUTPUT |= ( ((INPUT >> 3)&0b0000000000000001) << 7 ); 
00037     OUTPUT |= ( ((INPUT >> 4)&0b0000000000000001) << 6 ); 
00038     OUTPUT |= ( ((INPUT >> 5)&0b0000000000000001) << 5 ); 
00039     OUTPUT |= ( ((INPUT >> 6)&0b0000000000000001) << 4 ); 
00040     OUTPUT |= ( ((INPUT >> 7)&0b0000000000000001) << 3 ); 
00041     OUTPUT |= ( ((INPUT >> 8)&0b0000000000000001) << 2 ); 
00042     OUTPUT |= ( ((INPUT >> 9)&0b0000000000000001) << 1 ); 
00043     OUTPUT |= ( ((INPUT >>10)&0b0000000000000001) << 0 ); 
00044     OUTPUT = OUTPUT & 0b0000011111111111;
00045     return OUTPUT;
00046     
00047 }
00048 
00049 void ROBOFRIEN_SBUS::update(){
00050     while( SBUS_.readable() > 0){
00051         SBUS_ID[2] = SBUS_ID[1];
00052         SBUS_ID[1] = SBUS_ID[0];
00053         SBUS_ID[0] = BIT_REVERSE(SBUS_.getc());
00054         SBUS_CNT ++;
00055         if( start_bool == false ){
00056             if( ( SBUS_ID[1] == 0b00000000 ) & ( SBUS_ID[0] == 0b11110000 ) ){
00057                 start_bool = true;
00058                 check_cnt = SBUS_CNT;
00059                 SBUS_CNT = 0;
00060             }            
00061         }else{
00062             SBUS_BUFF[SBUS_CNT] = SBUS_ID[0];
00063             if(SBUS_CNT == 24){
00064                 start_bool = false;
00065                 channel[ 0]  = ( (uint16_t)(SBUS_BUFF[ 1]&0b11111111)<<3 | (uint16_t)((uint16_t)SBUS_BUFF[ 2]&0b11100000)>>5 );
00066                 channel[ 1]  = ( (int16_t)(SBUS_BUFF[ 2]&0b00011111)<<6 | (int16_t)(SBUS_BUFF[ 3]&0b11111100)>>2 );
00067                 channel[ 2]  = ( (int16_t)(SBUS_BUFF[ 3]&0b00000011)<<9 | (int16_t)(SBUS_BUFF[ 4]&0b11111111)<<1 | (int16_t)(SBUS_BUFF[5]&0b10000000)>>7 );
00068                 channel[ 3]  = ( (int16_t)(SBUS_BUFF[ 5]&0b01111111)<<4 | (int16_t)(SBUS_BUFF[ 6]&0b11110000)>>4 );
00069                 channel[ 4]  = ( (int16_t)(SBUS_BUFF[ 6]&0b00001111)<<7 | (int16_t)(SBUS_BUFF[ 7]&0b11111110)>>1 );
00070                 channel[ 5]  = ( (int16_t)(SBUS_BUFF[ 7]&0b00000001)<<10| (int16_t)(SBUS_BUFF[ 8]&0b11111111)<<2 | (int16_t)(SBUS_BUFF[9]&0b11000000)>>6 );
00071                 channel[ 6]  = ( (int16_t)(SBUS_BUFF[ 9]&0b00111111)<<5 | (int16_t)(SBUS_BUFF[10]&0b11111000)>>3 );
00072                 channel[ 7]  = ( (int16_t)(SBUS_BUFF[10]&0b00000111)<<8 | (int16_t)(SBUS_BUFF[11]&0b11111111)>>0 );
00073                 channel[ 8]  = ( (int16_t)(SBUS_BUFF[11]&0b11111111)<<3 | (int16_t)(SBUS_BUFF[12]&0b11100000)>>5 );
00074                 channel[ 9]  = ( (int16_t)(SBUS_BUFF[12]&0b00011111)<<6 | (int16_t)(SBUS_BUFF[13]&0b11111100)>>2 );
00075                 channel[10]  = ( (int16_t)(SBUS_BUFF[13]&0b00000011)<<9 | (int16_t)(SBUS_BUFF[14]&0b11111111)<<1 | (int16_t)(SBUS_BUFF[15]&0b10000000)>>7 );
00076                 channel[11]  = ( (int16_t)(SBUS_BUFF[15]&0b01111111)<<4 | (int16_t)(SBUS_BUFF[16]&0b11110000)>>4 );
00077                 channel[12]  = ( (int16_t)(SBUS_BUFF[16]&0b00001111)<<7 | (int16_t)(SBUS_BUFF[17]&0b11111110)>>1 );
00078                 channel[13]  = ( (int16_t)(SBUS_BUFF[17]&0b00000001)<<10| (int16_t)(SBUS_BUFF[18]&0b11111111)<<2 | (int16_t)(SBUS_BUFF[19]&0b11000000)>>6 );
00079                 channel[14]  = ( (int16_t)(SBUS_BUFF[19]&0b00111111)<<5 | (int16_t)(SBUS_BUFF[20]&0b11111000)>>3 );
00080                 channel[15]  = ( (int16_t)(SBUS_BUFF[20]&0b00000111)<<8 | (int16_t)(SBUS_BUFF[21]&0b11111111)>>0 );
00081                 for(int i=0; i<16; i++){
00082                     channel[ i]  = BIT11_REVERSE(channel[ i]);
00083                     if( channel[i] < CAP_MIN ) channel[i] = CAP_MIN;
00084                     else if( channel[i] > CAP_MAX ) channel[i] = CAP_MAX;
00085                     channel[i] = (channel[i] - 1024) * 1000 / 537.0;
00086                     // 487, 1024, 1561 //  RANGE : -1000 ~ 1000
00087                 }
00088                 if( channel[4] < -500 ){            // Motor OFF
00089                     CONTROLLER_STATE = MOTOR_OFF;
00090                 }else if( channel[4] < 500) {       // Manual Mode
00091                     if( channel[7] < -500){         // Manual - Attitude Mode
00092                         if( channel[2] < -950){
00093                             CONTROLLER_STATE = MOTOR_OFF;
00094                         }else{
00095                             CONTROLLER_STATE = MANUAL_ATTITUDE;                            
00096                         }
00097                     }else if( channel[7] < 500){    // Manual - Velocity Mode
00098                         CONTROLLER_STATE = MANUAL_VELOCITY;  
00099                     }else{                          // Manual - Position Mode
00100                         CONTROLLER_STATE = MANUAL_POSITION;                          
00101                     }
00102                 }else{                              // Auto Flight Mode
00103                     CONTROLLER_STATE = AUTO_FLIGHT;                                                                      
00104                     if( channel[ 5] < -500){
00105                         HOMEPOINT_BOOL = false;
00106                     }else if( channel[ 5] > 500){
00107                         HOMEPOINT_BOOL = true;
00108                     }else{
00109                         HOMEPOINT_BOOL = false;
00110                     }
00111                 }
00112             }
00113         }
00114     }
00115 }