Diff: src/d7a_com.cpp

Revision:

26:9f0b9833cac6

Parent:

25:aac250164497

Child:

27:934ab7455115

--- a/src/d7a_com.cpp	Fri Mar 25 16:48:02 2016 +0000
+++ b/src/d7a_com.cpp	Thu Mar 31 14:48:50 2016 +0000
@@ -99,6 +99,56 @@
     return;
 }
 
+// D7a_com constructor.
+// Opens a serial port and monitors the input for ALP packets.
+// Pins are those of the host, not the modem:
+// TX-host -> RX-modem
+// RX-host <- TX-modem
+// RTS-host -> CTS-modem
+// CTS-host <- RTS-modem
+void d7a_com_open( const d7a_com_config_t* config )
+{
+    FPRINT("\r\n");
+    
+    g_com_ctx.rx_buffer_size = config->rx_buffer_size;
+    g_com_ctx.rx_buffer = (uint8_t*)MALLOC(g_com_ctx.rx_buffer_size);
+    g_com_ctx.cts->rise(&cts_isr);
+    g_com_ctx.data_available = 0;
+    g_com_ctx.write_idx = 0;
+    g_com_ctx.read_idx = 0;
+    g_com_ctx.tx_seq = 0;
+    g_com_ctx.rx_seq = 0;
+    
+    g_com_ctx.serial =          new Serial(config->tx, config->rx);
+    g_com_ctx.rts =             new DigitalOut(config->rts);
+    g_com_ctx.cts =             new InterruptIn(config->cts);
+    g_com_ctx.data_parsing =    new Semaphore(1);
+    g_com_ctx.cts_int =         new Semaphore(1);
+    g_com_ctx.thread =          new Thread(d7a_com_thread, NULL, osPriorityBelowNormal, DEFAULT_STACK_SIZE*2);
+        
+    /* XXX: Unknown bug:
+    Baud rate can be found with high error rate (> 4%).
+    It is here manualy corrected after an oscilloscope measure.
+    Normal Baudrate should be 115200.
+    */
+    g_com_ctx.serial->baud(117000); // XXX
+    g_com_ctx.serial->format(8, SerialBase::None, 1);
+    g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq);
+}
+
+// Destructor
+void d7a_com_close()
+{
+    FPRINT("\r\n");
+    g_com_ctx.thread->terminate();
+    delete g_com_ctx.data_parsing;
+    delete g_com_ctx.cts_int;
+    delete g_com_ctx.serial;
+    delete g_com_ctx.rts;
+    delete g_com_ctx.cts;
+    FREE(g_com_ctx.rx_buffer);
+}
+
 
 /**
     Wakes-up modem and send data throught Serial.
@@ -107,7 +157,7 @@
     @param int                  Data length
     @return void
 */
-static void send(const uint8_t* buffer, int length)
+static void d7a_com_send(const uint8_t* buffer, int length)
 {    
     int i;
     
@@ -123,9 +173,43 @@
     *(g_com_ctx.rts) = 0;
 }
 
+// Formats and send packet throught Serial.
+void d7a_com_send_msg(d7a_com_tx_msg_t* msg)
+{
+    FPRINT("\r\n");
+    uint8_t* buf;
+    uint16_t len = msg->alen + msg->plen;
+    
+    buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len);
+    
+    // construct serial header
+    // concatenate and update tx_seq ID
+    buf[0] = (uint8_t)KAL_COM_SYNC_BYTE_0;
+    buf[1] = (uint8_t)KAL_COM_SYNC_BYTE_1;
+    buf[2] = (uint8_t)len;
+    buf[3] = (uint8_t)g_com_ctx.tx_seq++;
+    buf[4] = (uint8_t)msg->id;
+    len += KAL_COM_HEADER_LEN;
+
+    // copy payload and parameters
+    memcpy(buf + KAL_COM_HEADER_LEN, msg->pbuf, msg->plen);
+    memcpy(buf + KAL_COM_HEADER_LEN + msg->plen, msg->abuf, msg->alen);
+    
+    DPRINT("<-- %d (0x%02X)\r\n", len - KAL_COM_HEADER_LEN, msg->id);
+    
+    d7a_com_send(buf, len);
+    
+    // Stats
+    g_com_ctx.tx_pkt_count++;
+    g_com_ctx.tx_byte_count += len;
+    
+    FREE(buf);
+}
+
+
 static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt)
 {
-    FPRINT("\r\n");
+    //FPRINT("\r\n");
     // Distribute packet types to processes
     switch (KAL_COM_FLOWID(pkt->id))
     {
@@ -238,7 +322,7 @@
             data_read += i - 1;
             
             // add packet to queue
-            DPRINT("New packet SIZE:%d\r\n", pkt->blen);
+            DPRINT("--> %d (0x%02X)\r\n", pkt->blen, pkt->id);
             d7a_com_new_pkt(pkt);
             
             pkt_found = true;
@@ -290,91 +374,6 @@
 
 
 
-// D7a_com constructor.
-// Opens a serial port and monitors the input for ALP packets.
-// Pins are those of the host, not the modem:
-// TX-host -> RX-modem
-// RX-host <- TX-modem
-// RTS-host -> CTS-modem
-// CTS-host <- RTS-modem
-void d7a_com_open( const d7a_com_config_t* config )
-{
-    FPRINT("\r\n");
-    
-    g_com_ctx.rx_buffer_size = config->rx_buffer_size;
-    g_com_ctx.rx_buffer = (uint8_t*)MALLOC(g_com_ctx.rx_buffer_size);
-    g_com_ctx.cts->rise(&cts_isr);
-    g_com_ctx.data_available = 0;
-    g_com_ctx.write_idx = 0;
-    g_com_ctx.read_idx = 0;
-    g_com_ctx.tx_seq = 0;
-    g_com_ctx.rx_seq = 0;
-    
-    g_com_ctx.serial =          new Serial(config->tx, config->rx);
-    g_com_ctx.rts =             new DigitalOut(config->rts);
-    g_com_ctx.cts =             new InterruptIn(config->cts);
-    g_com_ctx.data_parsing =    new Semaphore(1);
-    g_com_ctx.cts_int =         new Semaphore(1);
-    g_com_ctx.thread =          new Thread(d7a_com_thread, NULL, osPriorityBelowNormal, DEFAULT_STACK_SIZE*2);
-        
-    /* XXX: Unknown bug:
-    Baud rate can be found with high error rate (> 4%).
-    It is here manualy corrected after an oscilloscope measure.
-    Normal Baudrate should be 115200.
-    */
-    g_com_ctx.serial->baud(117000); // XXX
-    g_com_ctx.serial->format(8, SerialBase::None, 1);
-    g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq);
-}
-
-// Destructor
-void d7a_com_close()
-{
-    FPRINT("\r\n");
-    g_com_ctx.thread->terminate();
-    delete g_com_ctx.data_parsing;
-    delete g_com_ctx.cts_int;
-    delete g_com_ctx.serial;
-    delete g_com_ctx.rts;
-    delete g_com_ctx.cts;
-    FREE(g_com_ctx.rx_buffer);
-}
-
-
-
-
-// Formats and send packet throught Serial.
-void d7a_com_send_msg(d7a_com_tx_msg_t* msg)
-{
-    FPRINT("\r\n");
-    uint8_t* buf;
-    uint16_t len = msg->alen + msg->plen;
-    
-    buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len);
-    
-    // construct serial header
-    // concatenate and update tx_seq ID
-    buf[0] = (uint8_t)KAL_COM_SYNC_BYTE_0;
-    buf[1] = (uint8_t)KAL_COM_SYNC_BYTE_1;
-    buf[2] = (uint8_t)len;
-    buf[3] = (uint8_t)g_com_ctx.tx_seq++;
-    buf[4] = (uint8_t)msg->id;
-    len += KAL_COM_HEADER_LEN;
-
-    // copy payload and parameters
-    memcpy(buf + KAL_COM_HEADER_LEN, msg->pbuf, msg->plen);
-    memcpy(buf + KAL_COM_HEADER_LEN + msg->plen, msg->abuf, msg->alen);
-    
-    //DPRINT("Sending %d bytes\r\n", len);
-    
-    send(buf, len);
-    
-    // Stats
-    g_com_ctx.tx_pkt_count++;
-    g_com_ctx.tx_byte_count += len;
-    
-    FREE(buf);
-}
 
 
 // Thread for parsing packets from RX buffer.
@@ -387,6 +386,5 @@
         g_com_ctx.data_parsing->wait();
         // search for packets
         parse_packet(g_com_ctx.read_idx, g_com_ctx.data_available);
-        FLUSH();
     }
 }