main.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01.h"
00003 #include "RF24.h"
00004 #include "RF24_config.h"
00005 
00006 Serial pc(PB_6, PB_7); //D5, D4 //PB_6, PB_7
00007 I2C i2c(PA_10, PA_9); //sda, scl PA_10, PA_9 D0, D1
00008 //(PinName mosi, PinName miso, PinName sck, PinName _cepin, PinName _csnpin)
00009 RF24 NRF24L01(PB_5, PB_4, PB_3, PA_12, PA_11);
00010 //RF24 NRF24L01(D11, D12, D13, D2, D10);
00011 
00012 AnalogIn JOYPIN1(PA_0); //A0
00013 AnalogIn JOYPIN2(PA_1); //A1
00014 AnalogIn JOYPIN3(PA_3); //A2
00015 AnalogIn JOYPIN4(PA_4); //A3
00016 
00017 AnalogIn POT1(PA_7); //A6
00018 AnalogIn POT2(PA_6); //A5
00019 AnalogIn POT3(PA_5); //A4
00020 
00021 DigitalIn BUTTON1(PA_2); //A7
00022 DigitalIn BUTTON2(PA_8); //D9
00023 
00024 DigitalOut MCU_LED(PB_0); //D3
00025 DigitalOut MCU_LED2(PB_1); //D6
00026 
00027 //MPU
00028 float mpu_temp_read;
00029 int32_t t_fine;
00030 int16_t gyro_raw[3], acc_raw[3], mag_raw[3];
00031 float gyro_offset[3], acc_offset[3];
00032 float mag_factory[3];
00033 int16_t angle[2];
00034 
00035 //magnetometer
00036 float x_offset = -1320.5, y_offset = 0, z_offset = -633.25;
00037 //float x_offset = -1320.5, y_offset = 1301.8, z_offset = -633.25;
00038 float x_scale = 0.9819, y_scale = 0.9957, z_scale = 1.0233;
00039 
00040 //RF
00041 const uint64_t pipe = 0x1212121212LL;
00042 float PITCH2 = 0, ROLL2 = 0, YAW2 = 0, THROTTLE2 = 0;
00043 int16_t PITCH = 0, ROLL = 0, YAW = 0, THROTTLE = 0;
00044 int8_t recv[30];
00045 int8_t trans[30];
00046 int8_t ackMessage[30];
00047 int16_t ackMessage2[30];
00048 uint32_t ackMessage3;
00049 int32_t ackMessage4[4];
00050 int8_t ackMessage5[3];
00051 
00052 //Timer Loop
00053 Timer timer1;
00054 
00055 int8_t TEMP_BUTTON1, TEMP_BUTTON2;
00056 int8_t temp_led = 1, temp_led2 = -1;
00057 int loop_count = 0, loop_count2 = 0, loop_count3 = 0, loop_count4 = 0, loop_count5 = 0; 
00058 int loop_count4_max = 50;
00059 float OFFSET1, OFFSET2, OFFSET3;
00060 
00061 int8_t current_mode = 0;
00062 
00063 int8_t toggle_mode = 1;
00064 
00065 //rf ack
00066 int8_t ack_count = 0;
00067 uint8_t cnt = 0;
00068 
00069 int32_t temp_time;
00070 uint8_t temp_time2;
00071 
00072 //-----------------------------------------------------------------------------------------
00073 
00074 int16_t constrain_int16(int16_t x, int16_t min, int16_t max)
00075 {
00076     if (x > max) x = max;
00077     else if (x < min) x = min;
00078     
00079     return x;
00080 }
00081 
00082 int32_t constrain_int32(int32_t x, int32_t min, int32_t max)
00083 {
00084     if (x > max) x = max;
00085     else if (x < min) x = min;
00086     
00087     return x;
00088 }
00089 
00090 float constrain_float(float x, float min, float max)
00091 {
00092     if (x > max) x = max;
00093     else if (x < min) x = min;
00094     
00095     return x;
00096 }
00097 
00098 //------------------------------------------------------------------------------------------------
00099 int main() {
00100     
00101     //disableDebugPorts();
00102     //pc.baud(230400); //115200
00103     //i2c.frequency(400000);
00104 
00105     //RF
00106     NRF24L01.begin();
00107     NRF24L01.setDataRate(RF24_2MBPS); //RF24_2MBPS
00108     NRF24L01.setChannel(22);
00109     NRF24L01.setPayloadSize(28); //28
00110     NRF24L01.setAddressWidth(5);
00111     NRF24L01.setRetries(2,4); //1,3 2,8
00112     
00113     NRF24L01.enableAckPayload();
00114     NRF24L01.openWritingPipe(pipe);
00115     pc.printf("start\n\r");
00116     int counta=0;
00117     timer1.start();
00118     
00119 //------------------------------------------------------------------------------------------------
00120     while(1) {
00121   
00122     loop_count = loop_count + 1;
00123     loop_count2 = loop_count2 + 1;
00124     loop_count4 = loop_count4 + 1;
00125 
00126     TEMP_BUTTON1 = (int8_t)BUTTON1;
00127     TEMP_BUTTON2 = (int8_t)BUTTON2;
00128 
00129         THROTTLE2 = JOYPIN1.read(); //left side vertical
00130         PITCH2 = JOYPIN3.read() - 0.5f; //65536 //up positive
00131         ROLL2 = JOYPIN4.read() - 0.5f; //right positive
00132         YAW2 = JOYPIN2.read() - 0.5f; // right positive
00133         
00134         OFFSET1 = POT1.read() - 0.5f; //left //reduce value counter-clockwise
00135         OFFSET2 = POT2.read() - 0.5f; //middle
00136         OFFSET3 = POT3.read() - 0.5f; //right
00137         //pc.printf("%f, %f, %f\n\r", OFFSET1, OFFSET2, OFFSET3);
00138         
00139         THROTTLE = (int16_t)(THROTTLE2 * 1024 * 1.15f);
00140         THROTTLE = constrain_int16(THROTTLE, 0, 1500);
00141         
00142         PITCH = -(int16_t)((PITCH2 * 1024  / 2 - OFFSET2 * 1024 / 4)); //-
00143         ROLL = -(int16_t)((ROLL2 * 1024 / 2 - OFFSET1 * 1024 / 4)); //-
00144         YAW = (int16_t)((- YAW2 * 1024 / 6 + OFFSET3 * 1024 / 2));
00145         
00146     if (loop_count >= 4){
00147         
00148         trans[0] = ((char *)&ROLL)[0];
00149         trans[1] = ((char *)&ROLL)[1];
00150         trans[2] = ((char *)&PITCH)[0]; //0;//
00151         trans[3] = ((char *)&PITCH)[1];
00152         trans[4] = ((char *)&YAW)[0];
00153         trans[5] = ((char *)&YAW)[1];
00154         trans[6] = ((char *)&THROTTLE)[0];
00155         trans[7] = ((char *)&THROTTLE)[1];
00156         trans[8] = (char)BUTTON1;
00157         trans[9] = (char)BUTTON2;
00158         printf("transmit\t%d",counta);
00159         counta++;
00160         NRF24L01.powerUp();
00161         NRF24L01.write(trans, 10);
00162         
00163 /*       
00164         if (NRF24L01.isAckPayloadAvailable()){
00165             NRF24L01.read(ackMessage, 14); //28 23
00166             
00167             ackMessage2[0] = *(int16_t*)(&ackMessage[0]); 
00168             ackMessage2[1] = *(int16_t*)(&ackMessage[2]); 
00169             ackMessage2[2] = *(int16_t*)(&ackMessage[4]); 
00170             
00171             ackMessage2[3] = *(int16_t*)(&ackMessage[6]);
00172             ackMessage2[4] = *(int16_t*)(&ackMessage[8]);
00173             
00174             ackMessage2[5] = *(int16_t*)(&ackMessage[10]);
00175             ackMessage2[6] = *(int16_t*)(&ackMessage[12]);
00176             
00177             temp_time2 = (uint8_t)(temp_time / 100); //1000
00178             
00179             temp_time = 0;
00180   
00181         }    
00182 */
00183 
00184     loop_count = 0;
00185     }
00186 
00187     if (loop_count4 >= loop_count4_max){
00188         
00189         if (temp_led == 1) {
00190             MCU_LED = 1;
00191             //MCU_LED2 = 0;
00192             }
00193         if (temp_led == -1) {
00194             MCU_LED = 0;
00195             //MCU_LED2 = 1;
00196             }
00197         temp_led = -1 * temp_led;  
00198         loop_count4 = 0;
00199     }
00200 
00201         while (timer1.read_us() < 2500); //400 Hz
00202         
00203         temp_time = temp_time + timer1.read_us();
00204         
00205         timer1.reset();
00206         
00207     }
00208     
00209 }
00210