Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: TXRX_serial.h
- Revision:
- 5:33abcc31b0aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TXRX_serial.h Mon May 12 22:58:06 2014 +0000 @@ -0,0 +1,150 @@ +#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; +} + + +//////////////////////////////////////////////////////