Diff: cmdmsg.cpp

Revision:

1:362331cec9b7

Child:

3:e5ea80fae61d

diff -r 887096209439 -r 362331cec9b7 cmdmsg.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cmdmsg.cpp	Thu Aug 20 07:41:02 2015 +0000
@@ -0,0 +1,317 @@
+#include "mbed.h"
+#include "cmdmsg.h"
+#include "ledstrip.h"
+//#define DETECT_LEDS
+#ifdef DETECT_LEDS
+#include "detectled.h"
+#endif
+//#define USE_SPIDEY_GEOM
+#ifdef USE_SPIDEY_GEOM
+#include "spideyGeometry.h"
+#endif
+/*
+
+Message format is:
+    <SEQ><CMD><CMD><CMD> ... <ZERO>
+    
+Where:
+    <SEQ> is a two byte sequence count
+    <CMD> is a series of bytes as follows:
+        <LEN><COD><PARAMS>
+        
+Where:
+    <LEN> is a two byte with value 1..65535 depicting msg len
+    <COD> is a one byte command code - see below
+    <PARAMS> are a series of bytes containing the parameters for the command
+    
+<COD> values:
+
+    NOTE MSB of COD is used to indicate that no response is required on success
+         i.e. if MSB of COD is 1 then no response is required on success
+         
+    (COD & 0x7f) ....
+    0x01:
+        Clear the entire strip
+    0x02:
+        Fill a series of leds with RGB values
+        <PARAMS> := <START><NUM><RGB1><RGB2>
+        Where:
+            <START> is a two byte value (bigendian) for led to start series
+            <NUM> is a two byte (bigendian) number of leds to set
+            <RGB1> is the starting RGB value (24bits)
+            <RGB2> is the ending RGB value (24bits) - OPTIONAL
+        The RGB2 value is optional - the LEN value determines whether it is present or not
+        - if present the colour values are interpolated (HSV) between RGB1 and RGB2
+        - if not present all leds are set to colour RGB1
+    0x03:
+        Detect LED with opto-sensor (put opto-sensor on LED and run command)
+        Sensor must be attached to p20 - currently using BPX65 with 470K pullup
+        Returns string with LED number
+    0x04:
+        Set number of leds and split point
+        <PARAMS> := <NUMLEDS><SPLITPOINT>
+        Where:
+            <NUMLEDS> is a two byte (bigendian) value for number of leds in strip
+            <SPLITPOINT> is a two byte (bigendian) value for point at which DigitalOut(p10) is used instead of p12 for data
+                    (this is because the spidey wall is two parallel chains of LEDS)
+    0x05:
+        Raw RGB values for a series of leds
+        <PARAMS> := <START><NUM><RGB1><RGB2><RGB2>....<RGBn>
+        Where:
+            <START> is a two byte value (bigendian) for led to start series
+            <NUM> is a two byte (bigendian) number of leds to set
+            <RGB1> is the RGB value for 1st LED (24bits)
+            <RGB2> is the RGB value for 2nd LED (24bits)
+            <RGBn> is the RGB value for nth LED (24bits)
+    0x06:
+        Set colour for leds on a pad
+        <PARAMS> := <PAD><RGB1><RGB2>
+        Where:
+            <PAD> is the pad number (1 byte)
+            <RGB1> is the starting RGB value (24bits)
+            <RGB2> is the ending RGB value (24bits) - OPTIONAL
+        The RGB2 value is optional - the LEN value determines whether it is present or not
+        - if present the colour values are interpolated (HSV) between RGB1 and RGB2
+        - if not present all leds are set to colour RGB1
+    0x07:
+        Set node and link info
+        <PARAMS> := <NODE><NODEFLAGS><NODERGB><LINKIDX><LINKLEDSTART><LINKNUMLEDS><LINKRGB>
+        Where:
+            <NODE> is the node number (1 byte)
+            <NODEFLAGS> LSB is 0 if the RGB value for the node is not to be changed, or 1 otherwise (1 byte)
+            <NODERGB> is the RBG value for the leds in the node itself (3 bytes)
+            <LINKIDX> is the index of the link (edge) from this node to control (1 byte)
+            <LINKLEDSTART> is the LED along the link to start controlling (1 byte)
+            <LINKNUMLEDS> is the number of LEDs to control (1 byte)
+            <LINKRGB> is the colour to make the link LEDs (3 bytes)
+        
+*/
+
+#ifdef USE_SPIDEY_GEOM
+int cmdmsg::getLedNumFromSeq(const int* edgeLeds, int numLedsInSeq, int seqIdx)
+{
+    if (seqIdx >= numLedsInSeq)
+        return -1;
+    int curSeqPos = 0;
+    bool reverseSeq = false;
+    for (int i = 0; i < MAX_SPIDEY_LINK_ELS/2; i++)
+    {
+        int startLedIdx = edgeLeds[i * 2];
+        int endLedIdx = edgeLeds[i * 2 + 1];
+        int nLeds = endLedIdx - startLedIdx + 1;
+        if (startLedIdx > endLedIdx)
+        {
+            reverseSeq = true;
+            nLeds = startLedIdx - endLedIdx + 1;
+        }
+        if (seqIdx - curSeqPos < nLeds)
+        {
+            if (reverseSeq)
+                return startLedIdx - seqIdx;
+            return startLedIdx + seqIdx;
+        }
+        curSeqPos = nLeds;
+    }
+    return -1;
+}
+#endif
+
+char* cmdmsg::Interpret(const unsigned char* msg, int msgLen, ledstrip* pLedStrip)
+{
+    // Get message details
+    static char responseStr[50];
+    unsigned char* ledStrip = pLedStrip->GetBuffer();
+    int nBytesLeft = msgLen - 2;
+    int seqCount = (msg[0] * 256) + msg[1];
+    msg += 2;
+    while (nBytesLeft > 0)
+    {
+        // Length of chunk
+        int chunkLen = (msg[0] * 256) + msg[1];
+        if (chunkLen == 0)
+            break;
+        
+        // Params length
+        int paramsLen = chunkLen - 1;
+//        printf("Chunklen = %d, Paramslen = %d\r\n", chunkLen, paramsLen);
+        
+        // Command and whether response required on success
+        int cmdCode = msg[2];
+        if ((cmdCode & 0x80) == 0)
+            sprintf(responseStr, "%04x OK", seqCount);
+        else
+            strcpy(responseStr, "");
+        
+        // Move message pointer to assist in decoding the payload
+        msg += 3;
+        switch(cmdCode & 0x7f)
+        {
+            case 0x01: // Clear
+            {
+//                printf("Clear - nextbyte %d\r\n", msg[0]);
+                pLedStrip->Clear();
+                break;
+            }
+            case 0x02: // Fill
+            {
+//                printf("Fill\r\n");
+
+                int startLed = (msg[0] * 256) + msg[1];
+                int numLeds = (msg[2] * 256) + msg[3];
+//                printf("F Q%04x S%d N%d Pa%d Ln%d\r\n", seqCount, startLed, numLeds, paramsLen, msgLen);
+                if (paramsLen == 10)  // RGB2 is provided
+                {
+                    pLedStrip->Fill(startLed, numLeds, msg[4], msg[5], msg[6], msg[7], msg[8], msg[9]);
+                } else if (paramsLen == 7) // only RGB1 - so solid colour fill
+                {
+                    pLedStrip->Fill(startLed, numLeds, msg[4], msg[5], msg[6]);
+                }
+                break;
+            }
+            case 0x03: // Detect LED
+            {
+#ifdef DETECT_LEDS
+                detectled ledDetector(p20);
+                int selectedLed = ledDetector.DetectSelectedLed(pLedStrip);
+                sprintf(responseStr, "%02x OK LED %d", seqCount, selectedLed);
+#endif
+                break;
+            }
+            case 0x04: // Set numleds and splitpoint
+            {
+//                printf("Set numleds and splitpoint\r\n");
+
+                if (paramsLen == 4)
+                {
+                    int numLeds = (msg[0] * 256) + msg[1];
+                    int splitPoint = (msg[2] * 256) + msg[3];
+                    pLedStrip->Resize(numLeds, splitPoint);
+                }
+                sprintf(responseStr, "%04x OK Resized", seqCount);
+                break;
+            }
+            case 0x05: // Raw - set LEDs to RGB values from buffer
+            {
+                if (paramsLen > 4)
+                {
+                    int startLed = (msg[0] * 256) + msg[1];
+                    int numLeds = (msg[2] * 256) + msg[3];
+                    if (paramsLen >= numLeds * 3 + 4)
+                    {
+                        pLedStrip->RawFill(startLed, numLeds, msg+4);
+//                        printf("R Q%04x S%d N%d Pa%d Ln%d\r\n", seqCount, startLed, numLeds, paramsLen, mMsgLen);
+                    }
+                    else
+                    {
+                        sprintf(responseStr, "%04x RAW FILL - BufLengthMismatch %d", seqCount, paramsLen);
+                    }
+                }
+                else
+                {
+                    sprintf(responseStr, "%04x RAW FILL - ParamsLen Error %d", seqCount, paramsLen);            
+                }
+                break;
+            }
+            case 0x06: // Set pad leds to a colour
+            {
+#ifdef USE_SPIDEY_GEOM
+                printf("Pad\r\n");
+
+                if (paramsLen > 2)
+                {
+                    int padIdx = msg[0];
+                    if (padIdx < sizeof(_spideyPads)/sizeof(SpideyPadInfo))
+                    {
+                        int firstLed = _spideyPads[padIdx].padLeds[0];
+                        int numLeds = _spideyPads[padIdx].padLeds[1] - _spideyPads[padIdx].padLeds[0] + 1;
+                        if (paramsLen == 4) // Only 1 RGB value
+                            pLedStrip->Fill(firstLed, numLeds, msg[1], msg[2], msg[3]);
+                        else if (paramsLen == 7) // Two RGB values
+                            pLedStrip->Fill(firstLed, numLeds, msg[1], msg[2], msg[3], msg[4], msg[5], msg[6]);
+                    }
+                }
+#endif
+                break;
+            }
+            case 0x07: // Control node
+            {
+#ifdef USE_SPIDEY_GEOM
+                printf("Node\r\n");
+
+                if (paramsLen == 11)
+                {
+                    int nodeIdx = msg[0];
+                    if (nodeIdx < sizeof(_spideyNodes)/sizeof(SpideyNodeInfo))
+                    {
+                        const SpideyNodeInfo& sni = _spideyNodes[nodeIdx];
+                        
+                        // Handle the node leds
+                        printf("Node %d leds %d colour %d,%d,%d ... ", nodeIdx, sni.numNodeLeds, msg[2], msg[3], msg[4]);
+                        bool setNodeRGB = ((msg[1] & 0x01) != 0);
+                        if (setNodeRGB)
+                        {
+                            for (int i = 0; (i < sni.numNodeLeds) && (i < MAX_SPIDEY_NODE_LEDS); i++)
+                            {
+                                pLedStrip->Fill(sni.nodeLeds[i], 1, msg[2], msg[3], msg[4]);
+                                printf("Led %d", sni.nodeLeds[i]);
+                            }
+                        }
+                        printf("\r\n");
+                        
+                        // Handle the link leds
+                        int linkIdx = msg[5];
+                        int linkLedStart = msg[6];
+                        int linkNumLeds = msg[7];
+                        printf("Link %d st %d num %d colour %d,%d,%d ... ", linkIdx, linkLedStart, linkNumLeds, msg[8], msg[9], msg[10]);
+                        if ((linkIdx < sni.numLinks) && (linkIdx < MAX_SPIDEY_NODE_LINKS))
+                        {
+                            const SpideyLinkInfo* pSli = sni.nodeLinks[linkIdx];
+                            for (int i = 0; i < linkNumLeds; i++)
+                            {
+                                int ledIdx = linkLedStart + i;
+                                int ledNumA = getLedNumFromSeq(pSli->edgeLedsA, pSli->edgeLengthA, ledIdx);
+                                if (ledNumA >= 0)
+                                {
+                                    pLedStrip->Fill(ledNumA, 1, msg[8], msg[9], msg[10]);
+                                    printf("A=%d/", ledNumA);
+                                }
+                                int ledNumB = getLedNumFromSeq(pSli->edgeLedsB, pSli->edgeLengthB, ledIdx);
+                                if (ledNumB >= 0)
+                                {
+                                    pLedStrip->Fill(ledNumB, 1, msg[8], msg[9], msg[10]);
+                                    printf("B=%d, ", ledNumB);
+                                }
+                            }
+                        }
+                        else
+                        {
+                            sprintf(responseStr, "%04x NODE - linkInvalid %d", seqCount, linkIdx);                    
+                        }
+                        printf("\r\n");
+                    }
+                    else
+                    {
+                        sprintf(responseStr, "%04x NODE - nodeInvalid %d", seqCount, nodeIdx);                    
+                    }
+                }
+                else
+                {
+                    sprintf(responseStr, "%04x NODE - BufLengthMismatch %d", seqCount, paramsLen);                    
+                }
+#endif
+                break;
+            }
+            default:
+            {
+                sprintf(responseStr, "%04x CMD %02x UNKNOWN", seqCount, cmdCode);
+                break;
+            }
+        }
+        
+        // Move to next chunk
+        nBytesLeft = nBytesLeft - chunkLen - 2;
+        msg += paramsLen;
+//        printf("Bytesleft = %d, msg[0] = %d\r\n", nBytesLeft, msg[0]);
+   }
+   return responseStr;
+}