Diff: dwm_uart.cpp

Revision:

0:bb732ce5e423

diff -r 000000000000 -r bb732ce5e423 dwm_uart.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dwm_uart.cpp	Mon Mar 25 16:32:43 2019 +0000
@@ -0,0 +1,152 @@
+#include "dwm_uart.h"
+//pour compter le temps entre 2 envoies de commande
+// le temps maximum le dwm1001 peut attendre est de 700us
+// ici, on met le temps timeout = 600us
+//dans le cas de reception, si il n'y a pas de reponse pendant 100ms, 
+//c'est un echec
+Timer timer; 
+
+
+dwm::dwm(PinName tx_pin, PinName rx_pin, int baudrate):
+    dwm_uart(tx_pin, rx_pin, baudrate)
+{
+    flushSerialBuffer();
+}
+
+int dwm::uart_write(uint8_t* tx_buffer, uint8_t len)
+{
+    
+    int i, begin, end;
+    timer.start();
+    
+    for(i=0; i<len; i++)
+    {
+        begin = timer.read_us();
+        //while(dwm_uart.writeable() == false){}
+        while(dwm_uart.writeable())
+        {
+            dwm_uart.putc(tx_buffer[i]);
+        }
+        end = timer.read_us();
+        wait_us(50);
+        if((end-begin)>DWM_WRITE_TIMEOUT) return RV_ERR; //echec
+    }
+    timer.stop();
+    return RV_OK; //reussi
+}
+
+void dwm::flushSerialBuffer(void) 
+{ 
+    while (dwm_uart.readable()) 
+    { 
+        dwm_uart.getc(); 
+    }
+}
+
+int dwm::uart_read(uint8_t* rx_buffer, int len_prevu)
+{
+    int i, len=0;
+    int begin, end;
+    timer.start();
+    begin = timer.read_ms();
+    //recuperer le type et la longeur des donnees
+    for(i=0; i<3; i++)
+    {
+        while(dwm_uart.readable() == false)
+        {
+            
+            end = timer.read_ms();
+            //wait_us(2);
+            if((end-begin)>DWM_READ_TIMEOUT) return RV_TIME; //echec
+                   
+        }
+        while(dwm_uart.readable())
+        {
+            rx_buffer[i] = dwm_uart.getc();
+            len = len + 1;
+        }
+    }
+    //recuperer des donnees si rx_buffer[2] == 0
+    if(rx_buffer[2] == 0)
+    {
+        for(int i=3; i<len_prevu; i++)
+        {
+            while(dwm_uart.readable() == false)
+            {
+                
+                end = timer.read_ms();
+                //wait_us(2);
+                if((end-begin)>DWM_READ_TIMEOUT) return RV_TIME; //echec
+                  
+            }
+            while(dwm_uart.readable())
+            {
+                rx_buffer[i] = dwm_uart.getc(); 
+                len = len + 1;
+            }
+        }
+    }
+    
+    timer.stop();
+    if(len != len_prevu) return RV_ERR; 
+    return RV_OK; 
+}
+
+int dwm::dwm_gpio_cfg_output()
+{
+    uint8_t tx_buffer[4];
+    uint8_t rx_buffer[255];
+    int rx_len;
+    //dwm_gpio_cfg_output
+    tx_buffer[0] = 0x28;
+    tx_buffer[1] = 0x02;
+    tx_buffer[2] = 0x0d;
+    tx_buffer[3] = 0x01;
+    uart_write(tx_buffer, 4);
+    wait_us(50);
+    rx_len = uart_read(rx_buffer,3);
+    if(rx_len == 0)
+    {
+        if(rx_buffer[0] != 0x40) return -1;
+        if(rx_buffer[1] != 0x01) return -2;
+        if(rx_buffer[2] != 0x00) return -3;
+    }
+    else return -1*rx_len;
+    return 0;
+}
+
+int dwm::dwm_pos_get()
+{
+    uint8_t TXbuffer[2];
+    uint8_t rx_buffer[255];
+    int i, ret;
+    //dwm_gpio_cfg_output
+    TXbuffer[0] = 0x02;
+    TXbuffer[1] = 0x00;
+    uart_write(TXbuffer, 2);
+    
+    ret = uart_read(rx_buffer, 18);
+    if(ret == RV_OK)
+    { 
+        i = 5;
+        node_pos.x =    rx_buffer[i]
+                  +(rx_buffer[i+1]<<8)
+                  +(rx_buffer[i+2]<<16)
+                  +(rx_buffer[i+3]<<24);
+        i += 4;
+        node_pos.y =    rx_buffer[i]
+                  +(rx_buffer[i+1]<<8)
+                  +(rx_buffer[i+2]<<16)
+                  +(rx_buffer[i+3]<<24);
+        i += 4;
+        node_pos.z =    rx_buffer[i]
+                  +(rx_buffer[i+1]<<8)
+                  +(rx_buffer[i+2]<<16)
+                  +(rx_buffer[i+3]<<24);
+        i += 4;
+        node_pos.qf =    rx_buffer[i];
+    }
+    else return ret;//***
+    
+    return ret;//****
+}
\ No newline at end of file