Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
TXRX_serial.h@5:33abcc31b0aa, 2014-05-12 (annotated)
- Committer:
- MatteoT
- Date:
- Mon May 12 22:58:06 2014 +0000
- Revision:
- 5:33abcc31b0aa
experiments (txrx and imu working)
Who changed what in which revision?
User | Revision | Line number | New 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 | ////////////////////////////////////////////////////// |