Diff: pixyUART.cpp

Revision:

5:d109b094d4bb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixyUART.cpp	Sat Aug 28 13:03:58 2021 +0000
@@ -0,0 +1,109 @@
+    
+
+#include "pixyUART.h"
+ 
+pixyUART::pixyUART(BufferedSerial *com)
+{
+    sync=0xAA55; //start word of a frame coming from the pixy
+    //blocks=new Block[nBlocks]; //allocate space for the blocks
+    //numBlocks=nBlocks;
+    numBlocks=NUM_BLOCKS;
+    uart = com;
+}
+ 
+pixyUART::~pixyUART()
+{
+    //delete blocks;
+}
+ 
+ 
+int8_t pixyUART::readTwoBytesLSB(uint16_t *out)
+{
+    int32_t num = uart->read(buffer, 2);
+    if(num>0)
+        {
+        *out=buffer[0]+256*buffer[1];
+        return 1;
+        }
+    else 
+        return -1;
+}
+ 
+ 
+void pixyUART::capture()
+{
+    memset(blocks, 0, numBlocks*sizeof(Block)); //destroy the old targets
+    captured_blocks = 0;
+    Timer ti;
+    ti.reset();
+    ti.start();
+    uint8_t got_data;
+    for (int i=0; i<numBlocks; i++) {
+        Block* out=&blocks[captured_blocks];
+        out->signature=INVALID_BLOCK;
+        uint16_t checksum=0;
+        //first we need to detect the start of a block. They all start with 0xAA55
+        char frame[2]= {0,0}; //this is a 2 byte running frame of what is being recieved. It's like a first-in-last-out queue
+        //debug->printf("looking for valid signature\n");
+        int32_t num = 1;
+        while(memcmp((char*)&sync, frame, 2)!=0) 
+            {
+            frame[0] = frame[1]; //move byte down
+            num = uart->read(&frame[1],1); //get next byte.
+            //printf("%d ",frame[1]);
+            if(num<=0)
+                {
+                break;
+                }
+            }
+        if(i==0)
+            ti.reset();
+        if(ti.read()>.015)
+            break;
+            //printf(" / ");
+        while(memcmp((char*)&sync, frame, 2)==0) 
+            {
+            num = uart->read(frame,2); //get next byte.
+            //printf("%d %d",frame[0],frame[1]);
+            if(num<=0)
+                {
+                break;
+                }
+            }
+            //printf(" . ");
+        checksum = frame[0]+256*frame[1];
+        got_data = readTwoBytesLSB(&out->signature);
+        if(got_data<0)
+            break;
+        got_data = readTwoBytesLSB(&out->x);
+        if(got_data<0)
+            break;
+        got_data = readTwoBytesLSB(&out->y);
+        if(got_data<0)
+            break;
+        got_data = readTwoBytesLSB(&out->width);
+        if(got_data<0)
+            break;
+        got_data = readTwoBytesLSB(&out->height);
+        if(got_data<0)
+            break;
+//printf("cs: %d i: %d x: %d y: %d w: %d h: %d\r\n",checksum,out->signature,out->x,out->y,out->width,out->height);
+        if (checksum!=(out->x+out->y+out->signature+out->width+out->height) || checksum==0) {
+            //debug->printf("checksum doesn't add up %d\n", checksum);
+            out->signature=INVALID_BLOCK; //used for invalid signatures
+        }
+        else
+            {
+                captured_blocks++;
+                }
+        
+    }
+    return;
+}
+ 
+Block* pixyUART::getBlocks()
+{
+    return blocks;
+}
+ 
+ 
\ No newline at end of file