Matteo Terruzzi / Mbed 2 deprecated MTQuadControl

Work in progress...

Dependencies:   ESC FreeIMU mbed-rtos mbed

Experiment - work in progress...

TXRX_serial.h@7:cda17cffec3c, 2014-05-24 (annotated)

Committer:
MatteoT
Date:
Sat May 24 17:42:03 2014 +0000
Revision:
7:cda17cffec3c
Parent:
5:33abcc31b0aa 
experiments (going to first launch)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 5:33abcc31b0aa 1 #define TXRX_BUFFER 600
MatteoT 5:33abcc31b0aa 2
MatteoT 5:33abcc31b0aa 3 Mutex TXRX_mutex;
MatteoT 5:33abcc31b0aa 4 QuadState TXRX_txQuadState;
MatteoT 5:33abcc31b0aa 5 QuadState TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 6 Timer TXRX_tx_dt_timer;
MatteoT 5:33abcc31b0aa 7 Timer TXRX_rx_dt_timer;
MatteoT 5:33abcc31b0aa 8 char TXRX_buffer [TXRX_BUFFER];
MatteoT 5:33abcc31b0aa 9
MatteoT 5:33abcc31b0aa 10
MatteoT 5:33abcc31b0aa 11 void TXRX_thread_routine (void const *args){ //New magic version
MatteoT 5:33abcc31b0aa 12 DigitalOut led_rx(LED_RX); led_rx = 0;
MatteoT 5:33abcc31b0aa 13
MatteoT 5:33abcc31b0aa 14 //Prepare state
MatteoT 5:33abcc31b0aa 15 TXRX_txQuadState.reset();
MatteoT 5:33abcc31b0aa 16 TXRX_rxQuadState.reset();
MatteoT 5:33abcc31b0aa 17
MatteoT 5:33abcc31b0aa 18 //Setup serial
MatteoT 5:33abcc31b0aa 19 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 20 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 21 RawSerial TXRX_serial(USBTX, USBRX);
MatteoT 5:33abcc31b0aa 22 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 23 TXRX_serial.baud(115200);
MatteoT 5:33abcc31b0aa 24 FAST_FLASH_OFF(led_rx,2);
MatteoT 5:33abcc31b0aa 25 Thread::wait(2000);
MatteoT 5:33abcc31b0aa 26
MatteoT 5:33abcc31b0aa 27 FAST_FLASH_ON(led_rx,5);
MatteoT 5:33abcc31b0aa 28 Thread::wait(5);
MatteoT 5:33abcc31b0aa 29
MatteoT 5:33abcc31b0aa 30
MatteoT 5:33abcc31b0aa 31 //Prepare timers
MatteoT 5:33abcc31b0aa 32 TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start();
MatteoT 5:33abcc31b0aa 33 TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start();
MatteoT 5:33abcc31b0aa 34
MatteoT 5:33abcc31b0aa 35 //Data to be setted into thread-safe area.
MatteoT 5:33abcc31b0aa 36 float actual_tx_dt, target_tx_dt, actual_rx_dt, target_rx_dt;
MatteoT 5:33abcc31b0aa 37
MatteoT 5:33abcc31b0aa 38 unsigned int step=0;
MatteoT 5:33abcc31b0aa 39 bool accept=false, sent=false;
MatteoT 5:33abcc31b0aa 40 while(1){
MatteoT 5:33abcc31b0aa 41
MatteoT 5:33abcc31b0aa 42 led_rx = (step % 40 < 35 ? 1 : (actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving.
MatteoT 5:33abcc31b0aa 43
MatteoT 5:33abcc31b0aa 44 accept = false;
MatteoT 5:33abcc31b0aa 45 if(TXRX_serial.readable()){
MatteoT 5:33abcc31b0aa 46 //RECEIVE ... [ ...read this with square brackets included... ] ...
MatteoT 5:33abcc31b0aa 47 bool read = false;
MatteoT 5:33abcc31b0aa 48 memset(TXRX_buffer,0,TXRX_BUFFER); //erase the buffer
MatteoT 5:33abcc31b0aa 49 char * receiving = TXRX_buffer; //receiving pointer
MatteoT 5:33abcc31b0aa 50 int byte; //received byte
MatteoT 5:33abcc31b0aa 51 unsigned int unreadable_count = 0;
MatteoT 5:33abcc31b0aa 52 while(receiving < TXRX_buffer+TXRX_BUFFER && unreadable_count < 1000){
MatteoT 5:33abcc31b0aa 53 if(TXRX_serial.readable()){
MatteoT 5:33abcc31b0aa 54 unreadable_count = 0;
MatteoT 5:33abcc31b0aa 55 byte = TXRX_serial.getc();
MatteoT 5:33abcc31b0aa 56 if(byte < 0)
MatteoT 5:33abcc31b0aa 57 break;
MatteoT 5:33abcc31b0aa 58 if(byte == '[')
MatteoT 5:33abcc31b0aa 59 read = true;
MatteoT 5:33abcc31b0aa 60 if(read){
MatteoT 5:33abcc31b0aa 61 *receiving = byte;
MatteoT 5:33abcc31b0aa 62 ++receiving;
MatteoT 5:33abcc31b0aa 63 if(byte == ']'){
MatteoT 5:33abcc31b0aa 64 accept = true;
MatteoT 5:33abcc31b0aa 65 break;
MatteoT 5:33abcc31b0aa 66 }
MatteoT 5:33abcc31b0aa 67 }
MatteoT 5:33abcc31b0aa 68 }else{
MatteoT 5:33abcc31b0aa 69 ++unreadable_count;
MatteoT 5:33abcc31b0aa 70 Thread::yield();
MatteoT 5:33abcc31b0aa 71 }
MatteoT 5:33abcc31b0aa 72 }
MatteoT 5:33abcc31b0aa 73 receiving = 0; //terminate the string
MatteoT 5:33abcc31b0aa 74 }
MatteoT 5:33abcc31b0aa 75 TXRX_mutex.lock();
MatteoT 5:33abcc31b0aa 76 //PRODUCE ACCEPTING DATA
MatteoT 5:33abcc31b0aa 77 if(accept && /*QuadState::length() == */TXRX_rxQuadState.setFromJSON(TXRX_buffer))
MatteoT 5:33abcc31b0aa 78 QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 79 else
MatteoT 5:33abcc31b0aa 80 QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer)
MatteoT 5:33abcc31b0aa 81 actual_rx_dt = TXRX_rxQuadState.actual_rx_dt;
MatteoT 5:33abcc31b0aa 82 target_rx_dt = TXRX_rxQuadState.target_rx_dt;
MatteoT 5:33abcc31b0aa 83
MatteoT 5:33abcc31b0aa 84 //TIME MEASURE SENT DATA
MatteoT 5:33abcc31b0aa 85 if(sent)
MatteoT 5:33abcc31b0aa 86 QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 87 else
MatteoT 5:33abcc31b0aa 88 QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer)
MatteoT 5:33abcc31b0aa 89 actual_tx_dt = TXRX_rxQuadState.actual_tx_dt;
MatteoT 5:33abcc31b0aa 90 target_tx_dt = TXRX_rxQuadState.target_tx_dt;
MatteoT 5:33abcc31b0aa 91
MatteoT 5:33abcc31b0aa 92 sent = false;
MatteoT 5:33abcc31b0aa 93 if(TXRX_serial.writeable()){
MatteoT 5:33abcc31b0aa 94 //PREPARE TO SEND
MatteoT 5:33abcc31b0aa 95 memset(TXRX_buffer,0,TXRX_BUFFER);
MatteoT 5:33abcc31b0aa 96 int setted = TXRX_txQuadState.getJSON(TXRX_buffer);
MatteoT 5:33abcc31b0aa 97 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 98 if(true || setted == QuadState::length()){
MatteoT 5:33abcc31b0aa 99 //SEND
MatteoT 5:33abcc31b0aa 100 unsigned int unwriteable_count=0;
MatteoT 5:33abcc31b0aa 101 unsigned int to_be_sent = strlen(TXRX_buffer);
MatteoT 5:33abcc31b0aa 102 char * sending = TXRX_buffer;
MatteoT 5:33abcc31b0aa 103 while(to_be_sent > 0 && unwriteable_count < 1000){
MatteoT 5:33abcc31b0aa 104 if(TXRX_serial.writeable()){
MatteoT 5:33abcc31b0aa 105 unwriteable_count = 0;
MatteoT 5:33abcc31b0aa 106 if(TXRX_serial.putc(*sending) < 0)
MatteoT 5:33abcc31b0aa 107 break;
MatteoT 5:33abcc31b0aa 108 ++sending;
MatteoT 5:33abcc31b0aa 109 --to_be_sent;
MatteoT 5:33abcc31b0aa 110 }else{
MatteoT 5:33abcc31b0aa 111 ++unwriteable_count;
MatteoT 5:33abcc31b0aa 112 Thread::yield();
MatteoT 5:33abcc31b0aa 113 }
MatteoT 5:33abcc31b0aa 114 }
MatteoT 5:33abcc31b0aa 115 sent = (to_be_sent == 0 ? true : false);
MatteoT 5:33abcc31b0aa 116 }
MatteoT 5:33abcc31b0aa 117 }
MatteoT 5:33abcc31b0aa 118
MatteoT 5:33abcc31b0aa 119 led_rx = 0;
MatteoT 5:33abcc31b0aa 120
MatteoT 5:33abcc31b0aa 121 ///REALX TIMINGS
MatteoT 5:33abcc31b0aa 122 //Sleep the shortest time needed to reach a dt_target; also sleep 30ms anyway.
MatteoT 5:33abcc31b0aa 123 double diff_tx = target_tx_dt - actual_tx_dt;
MatteoT 5:33abcc31b0aa 124 //double diff_rx = target_rx_dt - actual_rx_dt;
MatteoT 5:33abcc31b0aa 125 double diff_to_sleep = /*min(diff_rx,*/ diff_tx/*)*/ - 0.030;
MatteoT 5:33abcc31b0aa 126 if(diff_to_sleep > 0)
MatteoT 5:33abcc31b0aa 127 Thread::wait(diff_to_sleep);
MatteoT 5:33abcc31b0aa 128 Thread::wait(30); //NOTE: why?
MatteoT 5:33abcc31b0aa 129
MatteoT 5:33abcc31b0aa 130 ++step;
MatteoT 5:33abcc31b0aa 131 }
MatteoT 5:33abcc31b0aa 132 //end of while(1)
MatteoT 5:33abcc31b0aa 133 }
MatteoT 5:33abcc31b0aa 134
MatteoT 5:33abcc31b0aa 135
MatteoT 5:33abcc31b0aa 136 //////////////////////////////////////////////////////
MatteoT 5:33abcc31b0aa 137
MatteoT 5:33abcc31b0aa 138
MatteoT 5:33abcc31b0aa 139 bool TXRX_stateExchange (QuadState & tx, QuadState & rx){
MatteoT 5:33abcc31b0aa 140 if(TXRX_mutex.trylock()){
MatteoT 5:33abcc31b0aa 141 rx = TXRX_rxQuadState;
MatteoT 5:33abcc31b0aa 142 TXRX_txQuadState = tx;
MatteoT 5:33abcc31b0aa 143 TXRX_mutex.unlock();
MatteoT 5:33abcc31b0aa 144 return true;
MatteoT 5:33abcc31b0aa 145 }
MatteoT 5:33abcc31b0aa 146 return false;
MatteoT 5:33abcc31b0aa 147 }
MatteoT 5:33abcc31b0aa 148
MatteoT 5:33abcc31b0aa 149
MatteoT 5:33abcc31b0aa 150 //////////////////////////////////////////////////////