Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
TXRX_serial.h
- Committer:
- MatteoT
- Date:
- 2014-05-12
- Revision:
- 5:33abcc31b0aa
File content as of revision 5:33abcc31b0aa:
#define TXRX_BUFFER 600 Mutex TXRX_mutex; QuadState TXRX_txQuadState; QuadState TXRX_rxQuadState; Timer TXRX_tx_dt_timer; Timer TXRX_rx_dt_timer; char TXRX_buffer [TXRX_BUFFER]; void TXRX_thread_routine (void const *args){ //New magic version DigitalOut led_rx(LED_RX); led_rx = 0; //Prepare state TXRX_txQuadState.reset(); TXRX_rxQuadState.reset(); //Setup serial Thread::wait(2000); FAST_FLASH_OFF(led_rx,2); RawSerial TXRX_serial(USBTX, USBRX); FAST_FLASH_OFF(led_rx,2); TXRX_serial.baud(115200); FAST_FLASH_OFF(led_rx,2); Thread::wait(2000); FAST_FLASH_ON(led_rx,5); Thread::wait(5); //Prepare timers TXRX_tx_dt_timer.reset(); TXRX_tx_dt_timer.start(); TXRX_rx_dt_timer.reset(); TXRX_rx_dt_timer.start(); //Data to be setted into thread-safe area. float actual_tx_dt, target_tx_dt, actual_rx_dt, target_rx_dt; unsigned int step=0; bool accept=false, sent=false; while(1){ led_rx = (step % 40 < 35 ? 1 : (actual_rx_dt < 1 ? 1 : 0)); //allways light on while receiving, blink 0.5Hz while not receiving. accept = false; if(TXRX_serial.readable()){ //RECEIVE ... [ ...read this with square brackets included... ] ... bool read = false; memset(TXRX_buffer,0,TXRX_BUFFER); //erase the buffer char * receiving = TXRX_buffer; //receiving pointer int byte; //received byte unsigned int unreadable_count = 0; while(receiving < TXRX_buffer+TXRX_BUFFER && unreadable_count < 1000){ if(TXRX_serial.readable()){ unreadable_count = 0; byte = TXRX_serial.getc(); if(byte < 0) break; if(byte == '[') read = true; if(read){ *receiving = byte; ++receiving; if(byte == ']'){ accept = true; break; } } }else{ ++unreadable_count; Thread::yield(); } } receiving = 0; //terminate the string } TXRX_mutex.lock(); //PRODUCE ACCEPTING DATA if(accept && /*QuadState::length() == */TXRX_rxQuadState.setFromJSON(TXRX_buffer)) QUAD_STATE_UPDATE_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer) else QUAD_STATE_READ_ACTUAL_DT (TXRX_rxQuadState, rx, TXRX_rx_dt_timer) actual_rx_dt = TXRX_rxQuadState.actual_rx_dt; target_rx_dt = TXRX_rxQuadState.target_rx_dt; //TIME MEASURE SENT DATA if(sent) QUAD_STATE_UPDATE_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer) else QUAD_STATE_READ_ACTUAL_DT (TXRX_txQuadState, tx, TXRX_tx_dt_timer) actual_tx_dt = TXRX_rxQuadState.actual_tx_dt; target_tx_dt = TXRX_rxQuadState.target_tx_dt; sent = false; if(TXRX_serial.writeable()){ //PREPARE TO SEND memset(TXRX_buffer,0,TXRX_BUFFER); int setted = TXRX_txQuadState.getJSON(TXRX_buffer); TXRX_mutex.unlock(); if(true || setted == QuadState::length()){ //SEND unsigned int unwriteable_count=0; unsigned int to_be_sent = strlen(TXRX_buffer); char * sending = TXRX_buffer; while(to_be_sent > 0 && unwriteable_count < 1000){ if(TXRX_serial.writeable()){ unwriteable_count = 0; if(TXRX_serial.putc(*sending) < 0) break; ++sending; --to_be_sent; }else{ ++unwriteable_count; Thread::yield(); } } sent = (to_be_sent == 0 ? true : false); } } led_rx = 0; ///REALX TIMINGS //Sleep the shortest time needed to reach a dt_target; also sleep 30ms anyway. double diff_tx = target_tx_dt - actual_tx_dt; //double diff_rx = target_rx_dt - actual_rx_dt; double diff_to_sleep = /*min(diff_rx,*/ diff_tx/*)*/ - 0.030; if(diff_to_sleep > 0) Thread::wait(diff_to_sleep); Thread::wait(30); //NOTE: why? ++step; } //end of while(1) } ////////////////////////////////////////////////////// bool TXRX_stateExchange (QuadState & tx, QuadState & rx){ if(TXRX_mutex.trylock()){ rx = TXRX_rxQuadState; TXRX_txQuadState = tx; TXRX_mutex.unlock(); return true; } return false; } //////////////////////////////////////////////////////