Diff: common/CommandParser.cpp

Revision:

1:71c9c97c9f3d

Parent:

0:c347f602596d

Child:

2:a694440145e9

--- a/common/CommandParser.cpp	Sat Apr 20 09:23:39 2019 +0000
+++ b/common/CommandParser.cpp	Sun Apr 21 02:50:19 2019 +0000
@@ -41,17 +41,19 @@
     
     /** Command Format ":0 CMD 0000" */
     if (len != CmdLen) {
-        pUart->printf("[CommandParser::parse] Invalid Command Length\n");
+        //pUart->printf("[CommandParser::parse] Invalid Command Length\n");
+        this->reply(false, CommandParser::CmdReply::InvalidLen);
         return -1;
     }
     /** check Command Header */
     if (head[0] != ':') {
-        pUart->printf("[CommandParser::parse] Invalid Command\n");
+        //pUart->printf("[CommandParser::parse] Invalid Format\n");
+        this->reply(false, CommandParser::CmdReply::InvalidFmt);
         return -1;
     }
     /** check Command DeviceID */
     if ((head[1] - '0') != deviceID) {
-        pUart->printf("[CommandParser::parse] Ignore (DST is other)\n");
+        //pUart->printf("[CommandParser::parse] Ignore (DST is other)\n");
         return 0;
     }
     
@@ -65,9 +67,44 @@
             continue;
         }
         head += 4;
-        return (*ruleTable[rn].func)(head);
+        return (*ruleTable[rn].func)(this, head);
 
     }
-    pUart->printf("[CommandParser::parse] Invalid Command String\n");
+    //pUart->printf("[CommandParser::parse] Invalid Command");
+    this->reply(false, CommandParser::CmdReply::InvalidCmd);
     return -1;
+}
+
+int CommandParser::getDeviceID(void)
+{
+    return deviceID;
+}
+
+void CommandParser::reply(bool ack, int replyCode)
+{
+    char replyStr[] = "0000";
+    sprintf(replyStr, "%04d", replyCode);
+    this->reply(ack, replyStr);
+}
+
+void CommandParser::reply(bool ack, char* replyStr)
+{
+    const char CmdReplyStrDefault[] = ":0 ACK 0000\n";
+    const int CmdReplyLen = sizeof(CmdReplyStrDefault) /* include \0 */;
+    const int CmdReplyDevPos = 1;
+    const int CmdReplyAckPos = 3;
+    const int CmdReplyCodePos = 7;
+    const int CmdReplyEndPos = 11;
+    
+    char str[CmdReplyLen];
+    
+    /* set default handler */
+    memcpy(str, CmdReplyStrDefault, CmdReplyLen);
+    str[CmdReplyDevPos] = '0' + getDeviceID();
+    if (!ack) {
+        memcpy(&str[CmdReplyAckPos], "NAK", 3);
+    }
+    memcpy(&str[CmdReplyCodePos], replyStr, 4);
+    str[CmdReplyEndPos] = '\n';
+    pUart->printf(str);
 }
\ No newline at end of file