Diff: comms.cpp

Revision:

21:2fa676f214fe

Child:

22:9350752f5414

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/comms.cpp	Sat Jun 07 07:12:25 2014 +0000
@@ -0,0 +1,142 @@
+#include "comms.h"
+#include "debug.h"
+
+const int Packet::HEADER_SIZE = 5;
+
+/*  If successful, loads p with a new packet.  User is responsible for deleting.
+ *  Returns > 0 on success
+              0 on timeout
+            < 0 on error
+ */
+int Comms::receivePacket(Packet **p, int timeout) {
+//    PC_PRINTLN("receivePacket: getting header");
+    uint8_t headerBuf[Packet::HEADER_SIZE];
+    if (!getBytes(headerBuf, Packet::HEADER_SIZE, timeout))
+        return 0;
+    
+    uint8_t cmd = headerBuf[0];
+    uint16_t packetNumber = headerBuf[1];
+    uint16_t dataLength = headerBuf[3];
+    
+    PC_PRINTLNF("receivePacket: cmd = %d", cmd);
+    PC_PRINTLNF("receivePacket: packetNumber = %d", packetNumber);
+    PC_PRINTLNF("receivePacket: dataLength = %d", dataLength);
+    
+    uint8_t data[dataLength];
+    if (!getBytes(data, dataLength, timeout))
+        return 0;
+    
+    PC_PRINTLN("receivePacket: getting checksum");
+    uint16_t checkSum;
+    if (!getBytes((uint8_t *) &checkSum, sizeof(checkSum), timeout))
+        return 0;
+    
+    PC_PRINTLN("receivePacket: verifying checksum");
+    *p = Packet::createAndVerify(cmd, packetNumber, dataLength, data, checkSum);
+    if (*p == NULL)
+        return -1;
+    
+    return 1;
+}
+
+/* Sends packet p, and deletes it */
+void Comms::sendPacket(Packet *p) {
+    sendBytes((uint8_t *)&p->cmd, sizeof(p->cmd));
+    sendBytes((uint8_t *)&p->packetNumber, sizeof(p->packetNumber));
+    sendBytes((uint8_t *)&p->dataLength, sizeof(p->dataLength));
+    sendBytes((uint8_t *) p->data, p->dataLength);
+    sendBytes((uint8_t *)&p->checkSum, sizeof(p->checkSum));
+}
+
+void Comms::sendBytes(uint8_t *bytes, int nElms) {
+    for (int i = 0; i < nElms; i++)
+        bt.putc(bytes[i]);   
+}
+
+void Comms::sendByte(uint8_t byte) {
+    bt.putc(byte);
+}
+
+/* Loads buf with a byte from the bluetooth stream.
+ * Returns true if successful, false on timeout. */
+bool Comms::getBytes(uint8_t *buf, int n, int timeout) {
+    for (int i = 0; i < n; i++) {
+//        PC_PRINTLNF("getBytes: waiting for byte %d", i);
+        if (!getByte(buf+i, timeout)) {
+//            PC_PRINTLN("getBytes: failed to get byte");
+            return false;
+        }
+//        PC_PRINTLNF("getBytes: got byte %d", *(buf+i));
+    }
+    
+//    PC_PRINTLN("getBytes: got bytes");
+    return true;
+}
+
+/* Loads c with a byte from the bluetooth stream.
+ * Returns true if successful, false on timeout. */
+bool Comms::getByte(uint8_t *byte, int timeout) {
+//    PC_PRINTLN("getByte: getting byte");
+    if (timeout != INT_MAX)
+        timer.start();
+        
+//    PC_PRINTLN("getByte: waiting for bt readable");
+    // Spin until byte available or timeout
+    while ( !bt.readable() )
+        if (timer.read_ms() > timeout)
+            return false;
+    
+//    PC_PRINTLN("getByte: byte available");
+    timer.stop();
+    timer.reset();
+    *byte = bt.getc();
+    
+//    PC_PRINTLNF("getByte: byte read = %d", *byte);
+    return true;
+}
+
+/* Returns a new packet.  Copies dataLength bytes from data, and verifies the checkSum.
+ * If the given checkSum is incorrect, returns NULL.
+ * User is responsible for deleting the returned packet. */
+Packet *Packet::createAndVerify(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data, uint16_t checkSum) {
+    if (dataLength > 0 && data == NULL)
+        return NULL;
+    
+    Packet *p = new Packet(cmd, packetNumber, dataLength, data);
+    if (p->checkSum != checkSum)
+        return NULL;
+    else
+        return p;
+}
+
+/* Returns a new packet.  Copies dataLength bytes from data, and generates the checksum.
+ * User is responsible for deleting the returned packet. */
+Packet *Packet::create(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data) {
+    if (dataLength > 0 && data == NULL)
+        return NULL;
+        
+    return new Packet(cmd, packetNumber, dataLength, data);
+}
+
+/* Copies dataLength bytes from data */
+Packet::Packet(uint8_t cmd, uint16_t packetNumber, uint16_t dataLength, uint8_t *data) {
+    checkSum = 0;
+    
+    this->cmd = cmd;
+    this->packetNumber = packetNumber;
+    this->dataLength = dataLength;
+    
+    checkSum ^= cmd;
+    checkSum ^= packetNumber;
+    checkSum ^= dataLength;
+    
+    this->data = new uint8_t[dataLength];
+    for (int i = 0; i < dataLength; i++) {
+        this->data[i] = data[i];
+        checkSum ^= data[i];
+    }
+}
+
+Packet::~Packet() {
+    delete[] data;
+}
\ No newline at end of file