AT command firmware for MultiTech Dot devices.

Fork of mDot_AT_firmware by MultiTech

Dot Library Not Included!

Because these example programs can be used for both mDot and xDot devices, the LoRa stack is not included. The libmDot library should be imported if building for mDot devices. The libxDot library should be imported if building for xDot devices. The AT firmware was last tested with mbed-os-5.4.7. Using a version past mbed-os-5.4.7 will cause the build to fail. The library used with the AT firmware has to match the mbed-os version.

Dot Library Version 3 Updates

Dot Library versions 3.x.x require a channel plan to be injected into the stack. The Dot-Examples and Dot-AT-Firmware do this by defining a macro called "CHANNEL_PLAN" that controls the channel plan that will be used in the examples. Available channel plans will be in the Dot Library repository in the plans folder.

Revision 20 and earlier of Dot-Examples and revision 15 and earlier of Dot-AT-Firmware should be used with Dot Library versions prior to 3.0.0.

Fota Library

Th Fota Library must be added to compile for mDot 3.1.0 with Fota support. Latest dev libraries and 3.2.0 release will include Fota with libmDot/libxDot.

AT Firmware Description

This AT Firmware is what ships on mDot and xDot devices. It provides an AT command interface for using the mDot or xDot for LoRa communication.

AT command documentation can be found on Multitech.com.

The firmware changelog can be found here. The library changelog can be found here.

Dot Libraries

Dot Library Limitations

The commit messages in libmDot-mbed5 and libmDot-dev-mbed5 specify the version of the Dot library the commit contains and the version of mbed-os it was compiled against. We recommend building your application with the version of mbed-os specified in the commit message of the version of the Dot library you're using. This will ensure that you don't run into any runtime issues caused by differences in the mbed-os versions.

Stable and development libraries are available for both mDot and xDot platforms. The library chosen must match the target platform. Compiling for the mDot platform with the xDot library or vice versa will not succeed.

mDot Library

Development library for mDot.

libmDot-dev

Stable library for mDot.

libmDot

xDot Library

Development library for xDot.

libxDot-dev

Stable library for xDot.

libxDot

Revision:
28:c222ca8383f4
Parent:
27:5fafd3b26ac3
Child:
34:3b696c2b1e4b
--- a/CommandTerminal/CommandTerminal.cpp	Fri Sep 11 13:16:33 2020 -0500
+++ b/CommandTerminal/CommandTerminal.cpp	Thu Nov 19 09:58:25 2020 -0600
@@ -6,6 +6,8 @@
 #include "ChannelPlan.h"
 #include <cstdarg>
 #include <deque>
+#include "mts_at_version.h"
+
 #if defined(TARGET_XDOT_L151CC)
 #include "xdot_low_power.h"
 #endif
@@ -49,6 +51,7 @@
 
 static uint8_t f_data[252]; //max payload 242 plus 10 bytes for format
 static uint32_t _rxAddress = 0;
+static uint32_t _rxFcnt = 0;
 
 #if defined(TARGET_MTS_MDOT_F411RE)
 DigitalOut _packet_rx_pin(D12);
@@ -80,10 +83,53 @@
     _serialp = &serial;
 }
 
+void free_mem() {
+    // In order to get free mem within RTOS
+    // we need to get the main thread's stack pointer
+    // and subtract it with the top of the heap
+    // ------+-------------------+   Last Address of RAM (INITIAL_SP)
+    //       | Scheduler Stack   |
+    //       +-------------------+
+    //       | Main Thread Stack |
+    //       |         |         |
+    //       |         v         |
+    //       +-------------------+ <- bottom_of_stack/__get_MSP()
+    // RAM   |                   |
+    //       |  Available RAM    |
+    //       |                   |
+    //       +-------------------+ <- top_of_heap
+    //       |         ^         |
+    //       |         |         |
+    //       |       Heap        |
+    //       +-------------------+ <- __end__ / HEAP_START (linker defined var)
+    //       | ZI                |
+    //       +-------------------+
+    //       | ZI: Shell Stack   |
+    //       +-------------------+
+    //       | ZI: Idle Stack    |
+    //       +-------------------+
+    //       | ZI: Timer Stack   |
+    //       +-------------------+
+    //       | RW                |
+    // ------+===================+  First Address of RAM
+    //       |                   |
+    // Flash |                   |
+    //
+
+    uint32_t bottom_of_stack = __get_MSP();
+    char* top_of_heap =  (char *) malloc(sizeof(char));
+    uint32_t diff = bottom_of_stack - (uint32_t) top_of_heap;
+
+    free((void *) top_of_heap);
+
+    CommandTerminal::Serial()->writef("%lu bytes\r\n", diff);
+}
+
 void CommandTerminal::init() {
     _dot->setEvents(_events);
 }
 
+#if MTS_CMD_TERM_VERBOSE
 void CommandTerminal::printHelp() {
     const char* name = NULL;
     const char* text = NULL;
@@ -128,6 +174,7 @@
 
     write(newline);
 }
+#endif
 
 bool CommandTerminal::writeable() {
     return _serialp->writeable();
@@ -139,7 +186,7 @@
 
 char CommandTerminal::read() {
     char ch;
-    _serialp->read(&ch, 1);
+    _serialp->read(ch);
     return ch;
 }
 
@@ -165,18 +212,21 @@
     Timer serial_read_timer;
     std::vector<uint8_t> serial_buffer;
     std::vector<uint8_t> data;
-    int timeout = 0;
+    std::chrono::milliseconds timeout(0);
+    std::chrono::milliseconds elapsed_time_ms;
 
     serial_read_timer.start();
 
     if (_dot->getStartUpMode() == mDot::SERIAL_MODE) {
         _xbee_on_sleep = GPIO_PIN_SET;
 
-        timeout = _dot->getWakeDelay();
+        timeout = std::chrono::milliseconds(_dot->getWakeDelay());
+        elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(serial_read_timer.elapsed_time());
 
         // wait for timeout or start of serial data
-        while (!readable() && serial_read_timer.read_ms() < timeout && !_serialp->escaped()) {
-            osDelay(2);
+        while (!readable() && elapsed_time_ms < timeout && !_serialp->escaped()) {
+            ThisThread::sleep_for(2ms);
+            elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(serial_read_timer.elapsed_time());
         }
     }
 
@@ -188,15 +238,25 @@
     if (readable() && !_serialp->escaped()) {
 
         serial_read_timer.reset();
-        timeout = _dot->getWakeTimeout();
+        timeout = std::chrono::milliseconds(_dot->getWakeTimeout());
+
+        while (true) {
+            elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(serial_read_timer.elapsed_time());
+            if ((elapsed_time_ms >= timeout) ) {
 
-        while (serial_read_timer.read_ms() < timeout && serial_buffer.size() <= _dot->getNextTxMaxSize()) {
-            while (readable() && serial_buffer.size() < _dot->getNextTxMaxSize()) {
+                break;
+            }
+            if (serial_buffer.size() >= _dot->getNextTxMaxSize())  {
+                break;
+            }
+            if  (_serialp->escaped())  {
+                break;
+            }
+            if (readable()) {
                 serial_buffer.push_back(read());
                 serial_read_timer.reset();
-
-                if (_serialp->escaped())
-                    break;
+            } else {
+                ThisThread::sleep_for(5ms);
             }
         }
 
@@ -209,7 +269,7 @@
 L_SEND:
             // wait for any duty cycle limit to expire
             while (_dot->getNextTxMs() > 0 && !_serialp->escaped()) {
-                osDelay(10);
+                ThisThread::sleep_for(10ms);
             }
 
             if (_dot->getIsIdle()) {
@@ -217,6 +277,7 @@
                 if(_dot->getRxOutput() == mDot::EXTENDED || _dot->getRxOutput() == mDot::EXTENDED_HEX) {
                     formatPacketSDSend(serial_buffer);
                 }
+
                 if (_dot->send(serial_buffer, false) != mDot::MDOT_OK) {
                     logDebug("Send failed.");
                     // If the data should be tossed after send failure, clear buffer
@@ -227,7 +288,7 @@
 
                     // wait for send to finish
                     do {
-                        osDelay(50);
+                        ThisThread::sleep_for(50ms);
                     } while (!_dot->getIsIdle() && !_serialp->escaped());
 
                     // call recv to wait for any packet before sending again
@@ -257,7 +318,7 @@
                 }
             } else {
                 logDebug("Radio is busy, cannot send.\r\n");
-                osDelay(10);
+                ThisThread::sleep_for(10ms);
             }
 
         } else {
@@ -360,6 +421,36 @@
     return false;
 }
 
+#define CMD_DEFS_COUNT  (13)
+#define CMD_DEFS_VARIANT_SIZE  (4)
+#define CMD_DEFS_LABEL_SIZE  (7)
+
+struct CommandDefinition {
+    const char label[CMD_DEFS_LABEL_SIZE];
+    const char var[CMD_DEFS_VARIANT_SIZE];
+    uint8_t max_args;
+    CommandFactory::CmdId_t id;
+};
+
+// Table of commands handled locally
+static const CommandDefinition cmd_defs[CMD_DEFS_COUNT] = {
+    {"", "", 0, CommandFactory::eAT},
+    {"E", "?01", 0, CommandFactory::eATE},
+    {"V", "?01", 0, CommandFactory::eATVERBOSE},
+    {"&K", "?03", 0, CommandFactory::eATK},
+    {"+URC", "?", 1, CommandFactory::eURC},
+    {"+LW", "?", 0, CommandFactory::eLW},
+    {"+SD", "?", 0, CommandFactory::eSD},
+    {"&W", "", 0, CommandFactory::eATW},
+    {"&WP", "", 0, CommandFactory::eATWP},
+    {"+SS", "", 0, CommandFactory::eSS},
+    {"+DP", "?", 0, CommandFactory::eDP},
+    {"+SLEEP", "", 1, CommandFactory::eSLEEP},
+    {"+MEM", "?", 0, CommandFactory::eMEM}
+};
+
+
+
 void CommandTerminal::start() {
 
     char ch;
@@ -439,7 +530,13 @@
     }
     //Run terminal session
     while (running) {
-            // wait for input to reduce at command idle current
+
+        if (_events != NULL && CommandTerminal::Dot()->getTestModeEnabled() && _events->PacketReceived && _events->RxPort == 224) {
+            _events->handleTestModePacket();
+            return;
+        }
+
+        // wait for input to reduce at command idle current
         while (!readable() || _mode == mDot::SERIAL_MODE) {
             if (!join_canceled && _autoOTAEnabled) {
                 join_canceled = autoJoinCheck();
@@ -462,7 +559,7 @@
             ch = '\0';
 
             if (_mode != mDot::SERIAL_MODE) {
-                ThisThread::sleep_for(10); // 10 ms
+                ThisThread::sleep_for(10ms);
             }
             _serial.escaped();
         }
@@ -575,115 +672,239 @@
         }
 
         args[0] = mts::Text::toUpper(args[0]);
+        bool handled = false;
 
         // print help
         if ((args[0].find("?") == 0 || args[0].find("HELP") == 0) && args.size() == 1) {
+#if MTS_CMD_TERM_VERBOSE
             printHelp();
+#endif
             command.clear();
-        } else if ((args[0].find("ATE?") == 0 && args[0].length() == 4) || (args[0].find("ATE") == 0 && args[0].length() == 3)) {
-            writef("%d\r\n", _dot->getEcho());
-            write(done);
-        } else if (args[0].find("ATE0") == 0 && args[0].length() == 4) {
-            _dot->setEcho(false);
-            write(done);
-            echo = _dot->getEcho();
-        } else if (args[0].find("ATE1") == 0 && args[0].length() == 4) {
-            _dot->setEcho(true);
-            write(done);
-            echo = _dot->getEcho();
-        } else if ((args[0].find("ATV?") == 0 && args[0].length() == 4) || (args[0].find("ATV") == 0 && args[0].length() == 3)) {
-            writef("%d\r\n", _dot->getVerbose());
-            write(done);
-        } else if (args[0].find("ATV0") == 0 && args[0].length() == 4) {
-            _dot->setVerbose(false);
-            write(done);
-        } else if (args[0].find("ATV1") == 0 && args[0].length() == 4) {
-            _dot->setVerbose(true);
-            write(done);
-        } else if ((args[0].find("AT&K?") == 0 && args[0].length() == 5) || (args[0].find("AT&K") == 0 && args[0].length() == 4)) {
-            writef("%d\r\n", (_dot->getFlowControl() ? 3 : 0));
-            write(done);
-        } else if (args[0].find("AT&K0") == 0 && args[0].length() == 5) {
-            _dot->setFlowControl(false);
-            write(done);
-        } else if (args[0].find("AT&K3") == 0 && args[0].length() == 5) {
-            _dot->setFlowControl(true);
-            write(done);
-        } else if (args[0].find("AT+URC") == 0 && args[0].length() == 6) {
-            if (args.size() >= 2 && (args[1] != "?" && args[1] != "0" && args[1] != "1")) {
-                write("Invalid argument\r\n");
-                write(error);
-            } else if (args.size() == 2) {
-                if (args.size() > 1 && args[1] == "?") {
-                    write("(0:disable,1:enable)\r\n");
-                } else {
-                    urc_enabled = (args[1] == "1");
-                }
-            } else {
-                writef("%d\r\n", urc_enabled);
-            }
-            write(done);
-        } else if (args[0].find("AT+LW") == 0) {
-            if (args.size() >= 2 && (args[1] != "?")) {
-                write("Invalid argument\r\n");
-                write(error);
-            } else {
-                writef("%s\r\n", _dot->getMACVersion());
-                write(done);
-            }
-        } else if (args[0] == "AT+SD") {
-            if (_dot->getNetworkJoinStatus()) {
-                logDebug("Enter Serial Mode");
-                write(connect);
-                serial_data_mode = true;
-                _serialp->clearEscaped();
-                _mode = mDot::SERIAL_MODE;
-            } else {
-                logDebug("Network Not Joined");
-                write("Network Not Joined\r\n");
-                write(error);
-            }
-        } else if (args[0] == "AT+SLEEP") {
-            if (args.size() >= 2 && (args[1] != "?" && args[1] != "0" && args[1] != "1")) {
-                write("Invalid argument\r\n");
-                write(error);
-            } else {
-                if (args.size() > 1 && args[1] == "?") {
-                    write("(0:deepsleep,1:sleep)\r\n");
-                    write(done);
-                } else {
-                    _sleep_standby = !(args.size() > 1 && args[1] == "1");
-#if defined(TARGET_MTS_MDOT_F411RE)
-                    //Read the board ID. If all 0's, it is revision B. This hardware does not support deep sleep.
-                    DigitalIn ID2(PC_4);
-                    DigitalIn ID1(PC_5);
-                    DigitalIn ID0(PD_2);
-                    if(ID2 == 0 && ID1 == 0 && ID0 == 0 && _sleep_standby == 1){
-                        _sleep_standby = 0;
-                        logWarning("This hardware version does not support deep sleep. Using sleep mode instead.");
+            handled = true;
+        } else if (args[0].find("AT") == 0) {
+            const CommandDefinition* def = NULL;    // Command to handle if matched
+            char variant = '\0';                    // Variant character
+            for (int d = 0; (d < CMD_DEFS_COUNT) && (def == NULL); ++d) {
+                size_t label_size = 2 + strlen(cmd_defs[d].label);
+                if (args[0].find(cmd_defs[d].label) == 2) {
+                    // Label found following "AT"
+                    for (int v = 0; v < CMD_DEFS_VARIANT_SIZE; ++v) {
+                        if ((args[0][label_size] == cmd_defs[d].var[v]) &&
+                            (args[0][label_size] == '\0' || args[0][label_size + 1] == '\0')) {
+                            // Check for variant characters following label, this includes a NULL
+                            def = &cmd_defs[d];
+                            variant = cmd_defs[d].var[v];
+                            break;
+                        }
                     }
-#endif
-                    write(done);
-                    if (_dot->getBaud() < 9600)
-                        osDelay(20);
-                    else if (_dot->getBaud() > 57600)
-                        osDelay(2);
-                    else
-                        osDelay(5);
-                    this->sleep(_sleep_standby);
-                    osDelay(1);
                 }
             }
-        } else {
+            if (def != NULL) {
+                handled = true;
+                if (args.size() == 2 && args[1].length() == 1 && args[1][0] == '?') {
+                    handled = false;
+                } else if (args.size() - 1 > def->max_args) {
+#if MTS_CMD_TERM_VERBOSE
+                    write("Invalid argument\r\n");
+#endif
+                    write(error);
+                } else {
+                    switch (def->id) {
+                        case CommandFactory::eAT:
+                            write(done);
+                            break;
+                        case CommandFactory::eATE:
+                            if (variant == '1' || variant == '0') {
+                                _dot->setEcho(variant == '1');
+                                echo = _dot->getEcho();
+                            } else {
+                                writef("%d\r\n", _dot->getEcho());
+                            }
+                            write(done);
+                            break;
+                        case CommandFactory::eATVERBOSE:
+                            if (variant == '1' || variant == '0') {
+                                _dot->setVerbose(variant == '1');
+                            } else {
+                                writef("%d\r\n", _dot->getVerbose());
+                            }
+                            write(done);
+                            break;
+                        case CommandFactory::eATK:
+                            if (variant == '3' || variant == '0') {
+                                _dot->setFlowControl(variant == '3');
+                            } else {
+                                writef("%d\r\n", (_dot->getFlowControl() ? 3 : 0));
+                            }
+                            write(done);
+                            break;
+                        case CommandFactory::eURC:
+                            if (args.size() == 1) {
+                                writef("%d\r\n", urc_enabled);
+                                write(done);
+                            } else if (args[1].length() == 1) {
+                                if (args[1][0] != '?' && args[1][0] != '0' && args[1][0] != '1') {
+#if MTS_CMD_TERM_VERBOSE
+                                    write("Invalid argument\r\n");
+#endif
+                                    write(error);
+                                } else if (args[1][0] == '?') {
+#if MTS_CMD_TERM_VERBOSE
+                                    write("(0:disable,1:enable)\r\n");
+                                    write(done);
+#else
+                                    write(error);
+#endif
+                                } else {
+                                    urc_enabled = (args[1][0] == '1');
+                                    write(done);
+                                }
+                            } else {
+                                write(error);
+                            }
+                            break;
+                        case CommandFactory::eLW:
+                            writef("%s\r\n", _dot->getMACVersion());
+                            write(done);
+                            break;
+                        case CommandFactory::eMEM:
+                            free_mem();
+                            write(done);
+                            break;
+                        case CommandFactory::eSD:
+                            if (_dot->getNetworkJoinStatus()) {
+                                logDebug("Enter Serial Mode");
+                                write(connect);
+                                serial_data_mode = true;
+                                _serialp->clearEscaped();
+                                _mode = mDot::SERIAL_MODE;
+                            } else {
+                                logDebug("Network Not Joined");
+                                write("Network Not Joined\r\n");
+                                write(error);
+                            }
+                            break;
+                        case CommandFactory::eATW:
+                            if (!_dot->saveConfig()) {
+#if MTS_CMD_TERM_VERBOSE
+                                write("Failed to save to flash");
+#endif
+                                write(error);
+                            } else {
+                                write(done);
+                            }
+                            break;
+                        case CommandFactory::eATWP:
+                            if (!_dot->saveProtectedConfig()) {
+#if MTS_CMD_TERM_VERBOSE
+                                write("Failed to save to flash");
+#endif
+                                write(error);
+                            } else {
+                                write(done);
+                            }
+                            break;
+                        case CommandFactory::eSS:
+                            _dot->saveNetworkSession();
+                            write(done);
+                            break;
+                        case CommandFactory::eDP:
+                            writef("%d\r\n", _dot->getDataPending());
+                            write(done);
+                            break;
+                        case CommandFactory::eSLEEP: {
+                            bool temp_sleep_standby;
+                            bool valid = false;
+                            if ((args.size() > 1) && (args[1].length() == 1)) {
+                                if ((args[1][0] != '?' && args[1][0] != '0' && args[1][0] != '1')) {
+#if MTS_CMD_TERM_VERBOSE
+                                    write("Invalid argument\r\n");
+#endif
+                                    write(error);
+                                } else if (args[1][0] == '?') {
+#if MTS_CMD_TERM_VERBOSE
+                                    write("(0:deepsleep,1:sleep)\r\n");
+                                    write(done);
+#else
+                                    write(error);
+#endif
+                                } else {
+                                    valid = true;
+                                    temp_sleep_standby = (args[1][0] == '0');
+                                }
+                            } else if (args.size() == 1) {
+                                valid = true;
+                                temp_sleep_standby = _sleep_standby;
+                            } else {
+                                write(error);
+                            }
+
+                            if (valid) {
+                                if (!_dot->getIsIdle()) {
+                                    write("Dot is not idle\r\n");
+                                    write(error);
+                                } else {
+                                    _sleep_standby = temp_sleep_standby;
+#if defined(TARGET_MTS_MDOT_F411RE)
+                                    //Read the board ID. If all 0's, it is revision B. This hardware does not support deep sleep.
+                                    DigitalIn ID2(PC_4);
+                                    DigitalIn ID1(PC_5);
+                                    DigitalIn ID0(PD_2);
+                                    if(ID2 == 0 && ID1 == 0 && ID0 == 0 && _sleep_standby == 1){
+                                        _sleep_standby = 0;
+                                        logWarning("This hardware version does not support deep sleep. Using sleep mode instead.");
+                                    }
+#endif
+                                    write(done);
+                                    if (_dot->getBaud() < 9600)
+                                        osDelay(20);
+                                    else if (_dot->getBaud() > 57600)
+                                        osDelay(2);
+                                    else
+                                        osDelay(5);
+                                    this->sleep(_sleep_standby);
+                                    osDelay(1);
+                                }
+                            }
+                            break;
+                        }
+                        default:
+                            handled = false;
+                            // Unhandled command
+                            // Will only reach here if the table contains commands that do not have explicit cases
+                            break;
+                    }
+                }
+#ifdef MTS_RADIO_DEBUG_COMMANDS
+            } else if (args[0].find("AT+TM!") == 0 && args[0].length() == 6) {
+                handled = true;
+                if ((args.size() > 1) && (args[1].length() == 1)) {
+                    if (args[1][0] == '0' || args[1][0] == '1') {
+                        _dot->setTestModeEnabled(args[1][0] == '1');
+                        write(done);
+                    } else {
+                        write(error);
+                    }
+                } else {
+                    writef("%d\r\n", _dot->getTestModeEnabled());
+                    write(done);
+                }
+#endif
+            }
+        }
+
+        if (!handled) {
             bool found = false;
             bool query = false;
 
             std::string lookfor = args[0];
 
+#if MTS_CMD_TERM_VERBOSE
             // per command help
-            if ((args[0].find("?") == 0 || args[0].find("HELP") == 0))
+            if ((args[0].find("?") == 0 || args[0].find("HELP") == 0)) {
                 lookfor = mts::Text::toUpper(args[1]);
-
+            }
+#endif
             // trim off any trailing '?' and mark as a query command
             if (args[0].rfind("?") == args[0].length() - 1) {
                 query = true;
@@ -698,36 +919,60 @@
                 // match CMD or CMD? syntax if command is queryable
                 if (lookfor == cmd->text() && (!query || (query && cmd->queryable()))) {
                     found = true;
+                    _errorMessage.clear();
+#if MTS_CMD_TERM_VERBOSE
                     if (args[0] == "HELP") {
                         writef("%s%s", cmd->help().c_str(), newline);
                         write(done);
                     }
-
                     else if (args.size() > 1 && args[1] == "?") {
                         writef("%s%s", cmd->usage().c_str(), newline);
                         write(done);
                     } else if (!cmd->verify(args)) {
-                        writef("%s%s", _errorMessage.c_str(), newline);
-                        writef("%s", error);
+#else
+                    if (args.size() > 1 && args[1] == "?") {
+                        write(error);
+                    } else if (!cmd->verify(args)) {
+#endif
+                        if (!_errorMessage.empty()) {
+                            writef("%s%s", _errorMessage.c_str(), newline);
+                        }
+                        write(error);
                     } else {
+                        _errorMessage.clear();
                         if (cmd->action(args) == 0) {
                             writef("%s", done);
                         } else {
-                            writef("%s%s", _errorMessage.c_str(), newline);
-                            writef("%s", error);
+                            // Action was not successful
+                            if (_errorMessage.empty()) {
+                                // If no error message was set, check for error recorded in mdot
+                                std::string dot_error = _dot->getLastError();
+                                if (!dot_error.empty()) {
+                                    writef("%s%s", dot_error.c_str(), newline);
+                                }
+                            } else {
+                                // Command set an error message
+                                writef("%s%s", _errorMessage.c_str(), newline);
+                            }
+                            write(error);
                         }
                     }
                 }
 
                 delete cmd;
+
+                if (found) {
+                    break;
+                }
             }
 
             if (!found) {
-                writef("%s", command_error);
-                writef("%s", error);
+                write(command_error);
+                write(error);
             }
         }
 
+
         _join_status_pin = CommandTerminal::Dot()->getNetworkJoinStatus();
         command_processing = false;
 
@@ -1056,9 +1301,10 @@
     _packet_rx_pin = 0;
 }
 
-void CommandTerminal::RadioEvent::PacketRx(uint8_t port, uint8_t *payload, uint16_t size, int16_t rssi, int16_t snr, lora::DownlinkControl ctrl, uint8_t slot, uint8_t retries, uint32_t address, bool dupRx) {
-    mDotEvent::PacketRx(port, payload, size, rssi, snr, ctrl, slot, retries, address, dupRx);
+void CommandTerminal::RadioEvent::PacketRx(uint8_t port, uint8_t *payload, uint16_t size, int16_t rssi, int16_t snr, lora::DownlinkControl ctrl, uint8_t slot, uint8_t retries, uint32_t address, uint32_t fcnt, bool dupRx) {
+    mDotEvent::PacketRx(port, payload, size, rssi, snr, ctrl, slot, retries, address, fcnt, dupRx);
     _rxAddress = address;
+    _rxFcnt = fcnt;
 
     if(port == 200 || port == 201 || port == 202) {
         Fota::getInstance()->processCmd(payload, port, size);
@@ -1090,12 +1336,221 @@
             if (CommandTerminal::Dot()->getRxOutput() >= mDot::EXTENDED) {
                 formatPacket(RxPayload, size, CommandTerminal::Dot()->getRxOutput() == mDot::EXTENDED_HEX);
             } else {
-                CommandTerminal::Serial()->writef("RECV\r\n");
+                CommandTerminal::Serial()->write("RECV\r\n", 6);
             }
         }
     }
 }
 
+void CommandTerminal::RadioEvent::handleTestModePacket() {
+#ifdef MTS_RADIO_DEBUG_COMMANDS
+    static uint32_t last_rx_seq = 0;
+    bool start_test = false;
+
+    std::string packet = mts::Text::bin2hexString(RxPayload, RxPayloadSize);
+
+    start_test = RxPayloadSize == 4;
+    start_test = start_test && (RxPayload[0] == 0x01);
+    start_test = start_test && (RxPayload[1] == 0x01);
+    start_test = start_test && (RxPayload[2] == 0x01);
+    start_test = start_test && (RxPayload[3] == 0x01);
+
+    if (start_test) {
+        CommandTerminal::Dot()->getSettings()->Test.TestMode = true;
+
+        last_rx_seq = CommandTerminal::Dot()->getSettings()->Session.UplinkCounter;
+
+        uint16_t testDownlinkCounter = 0;
+        uint32_t txPeriod = 5000;
+        bool testConfirmed = false;
+
+        // init counter
+        std::vector<uint8_t> data;
+        uint8_t savedPort = CommandTerminal::Dot()->getAppPort();
+        uint8_t savedAcks = CommandTerminal::Dot()->getAck();
+        bool savedAdr = CommandTerminal::Dot()->getAdr();
+
+        CommandTerminal::Dot()->setAck(0);
+        CommandTerminal::Dot()->setAdr(true);
+        CommandTerminal::Dot()->setAppPort(224);
+
+        Timer sentTimer;
+        sentTimer.start();
+        while (CommandTerminal::Dot()->getSettings()->Test.TestMode) {
+TEST_START:
+            if (RxPort == 224) {
+                data.clear();
+
+                switch (RxPayload[0]) {
+                    case 0x00: { // PackageVersionReq
+                        data.push_back(0x00);
+                        data.push_back(0x06);
+                        data.push_back(0x01);
+                        break;
+                    }
+                    case 0x01: { // DutResetReq
+                        if (RxPayloadSize == 1) {
+                            CommandTerminal::Dot()->resetCpu();
+                        }
+                        break;
+                    }
+                    case 0x02: { // DutJoinReq
+                        if (RxPayloadSize == 1) {
+                            CommandTerminal::Dot()->joinNetworkOnce();
+                        }
+                        break;
+                    }
+                    case 0x03: { // SwitchClassReq
+                        std::string cls = "A";
+                        if (RxPayload[1] > 0 && RxPayload[1] < 3) {
+                            cls = RxPayload[1] == 0x01 ? "B" : "C";
+                        }
+                        CommandTerminal::Dot()->setClass(cls);
+                        break;
+                    }
+                    case 0x04: { // ADR Enabled/Disable
+                        if (RxPayload[1] == 1)
+                            CommandTerminal::Dot()->setAdr(true);
+                        else
+                            CommandTerminal::Dot()->setAdr(false);
+                        break;
+                    }
+                    case 0x05: { // RegionalDutyCycleCtrlReq
+                        if (RxPayload[1] == 0)
+                            CommandTerminal::Dot()->setDisableDutyCycle(true);
+                        else
+                            CommandTerminal::Dot()->setDisableDutyCycle(false);
+
+                        break;
+                    }
+                    case 0x06: { // TxPeriodicityChangeReq
+                        if (RxPayload[1] < 2)
+                            // 0, 1 => 5s
+                            txPeriod = 5000U;
+                        else if (RxPayload[1] < 8)
+                            // 2 - 7 => 10s - 60s
+                            txPeriod = (RxPayload[1] - 1) * 10000U;
+                        else if (RxPayload[1] < 11) {
+                            // 8, 9, 10 => 120s, 240s, 480s
+                            txPeriod = 120 * (1 << (RxPayload[1] - 8)) * 1000U;
+                        }
+                        break;
+                    }
+                    case 0x07: { // TxFramesCtrl
+                        if (RxPayload[1] == 0) {
+                            // NO-OP
+                        } else if (RxPayload[1] == 1) {
+                            testConfirmed = false;
+                            CommandTerminal::Dot()->getSettings()->Network.AckEnabled = 0;
+                            CommandTerminal::Dot()->getSettings()->Session.Redundancy = 0;
+                        } else if (RxPayload[1] == 2) {
+                            testConfirmed = true;
+                            CommandTerminal::Dot()->getSettings()->Network.AckEnabled = 8;
+                            CommandTerminal::Dot()->getSettings()->Session.Redundancy = 8;
+                        }
+                        break;
+                    }
+                    case 0x08: { // EchoPayloadReq
+                        data.push_back(0x08);
+                        for (size_t i = 1; i < RxPayloadSize; i++) {
+                            data.push_back(RxPayload[i] + 1);
+                        }
+                        break;
+                    }
+                    case 0x09: { // RxAppCntReq
+                        data.push_back(0x09);
+                        data.push_back(testDownlinkCounter >> 8);
+                        data.push_back(testDownlinkCounter & 0xFF);
+                        break;
+                    }
+                    case 0x0A: { // RxAppCntResetReq
+                        testDownlinkCounter = 0;
+                        break;
+                    }
+                    case 0x20: { // LinkCheckReq
+                        CommandTerminal::Dot()->addMacCommand(lora::MOTE_MAC_LINK_CHECK_REQ, 0, 0);
+                        break;
+                    }
+                    case 0x21: { // DeviceTimeReq
+                        CommandTerminal::Dot()->addDeviceTimeRequest();
+                        break;
+                    }
+                    case 0x22: { // PingSlotInfo
+                        CommandTerminal::Dot()->setPingPeriodicity(RxPayload[1]);
+                        CommandTerminal::Dot()->addMacCommand(lora::MOTE_MAC_PING_SLOT_INFO_REQ, RxPayload[1], 0);
+                        break;
+                    }
+                    case 0x7D: { // TxCw
+                        uint32_t freq = 0;
+                        uint16_t timeout = 0;
+                        uint8_t power = 0;
+
+                        timeout = RxPayload[1] << 8 | RxPayload[2];
+                        freq = (RxPayload[3] << 16 | RxPayload[4] << 8 | RxPayload[5]) * 100;
+                        power = RxPayload[6];
+
+                        CommandTerminal::Dot()->sendContinuous(true, timeout * 1000, freq, power);
+                        break;
+                    }
+                    case 0x7E: { // DutFPort224DisableReq
+                        _dot->setTestModeEnabled(false);
+                        CommandTerminal::Dot()->getSettings()->Test.TestMode = false;
+                        _dot->saveConfig();
+                        break;
+                    }
+                    case 0x7F: { // DutVersionReq
+                        std::string version = AT_APPLICATION_VERSION;
+                        int temp = 0;
+
+                        data.push_back(0x7F);
+                        int ret = sscanf(&version[0], "%d", &temp);
+                        data.push_back(temp); // AT_APP_VERSION_MAJOR; // MAJOR
+                        ret = sscanf(&version[2], "%d", &temp);
+                        data.push_back(temp); // AT_APP_VERSION_MINOR; // MINOR
+                        ret = sscanf(&version[4], "%d", &temp);
+                        data.push_back(temp); // AT_APP_VERSION_PATCH; // PATCH
+                        break;
+                    }
+                    default: {
+                        break;
+                    }
+                }
+            }
+
+            do {
+                if (std::chrono::duration_cast<std::chrono::milliseconds>(sentTimer.elapsed_time()).count() < txPeriod)
+                    osDelay(txPeriod - std::chrono::duration_cast<std::chrono::milliseconds>(sentTimer.elapsed_time()).count());
+
+                sentTimer.reset();
+
+                if (CommandTerminal::Dot()->send(data, testConfirmed) == mDot::MDOT_MAX_PAYLOAD_EXCEEDED) {
+                    data.clear();
+                    RxPort = 0;
+                    goto TEST_START;
+                }
+
+                if (PacketReceived) {
+                    last_rx_seq = CommandTerminal::Dot()->getSettings()->Session.UplinkCounter;
+                } else if ( CommandTerminal::Dot()->getSettings()->Session.UplinkCounter - last_rx_seq > 160) {
+                    CommandTerminal::Dot()->setAck(savedAcks);
+                    CommandTerminal::Dot()->setAppPort(savedPort);
+                    CommandTerminal::Dot()->setAdr(savedAdr);
+                    CommandTerminal::Dot()->getSettings()->Test.TestMode = false;
+                    start_test = false;
+                    break;
+                }
+
+                data.clear();
+
+            } while (CommandTerminal::Dot()->recv(data) != mDot::MDOT_OK);
+
+            if (RxPort != 0 || RxPayloadSize == 0)
+                testDownlinkCounter++;
+        }
+    }
+#endif
+}
+
 uint8_t CommandTerminal::getBatteryLevel() {
     return _battery_level;
 }
@@ -1105,7 +1560,6 @@
 }
 
 void CommandTerminal::formatPacket(uint8_t* payload, uint16_t size, bool hex) {
-    uint32_t fcnt = _dot->getDownLinkCounter();
 
     if(_dot->getAckRequested()) {
         f_data[0] = 0;
@@ -1117,10 +1571,10 @@
     f_data[2] = (_rxAddress >> 8) & 0xFF;
     f_data[3] = (_rxAddress >> 16) & 0xFF;
     f_data[4] = (_rxAddress >> 24) & 0xFF;
-    f_data[5] = fcnt & 0xFF;
-    f_data[6] = (fcnt >> 8) & 0xFF;
-    f_data[7] = (fcnt >> 16) & 0xFF;
-    f_data[8] = (fcnt >> 24) & 0xFF;
+    f_data[5] = _rxFcnt & 0xFF;
+    f_data[6] = (_rxFcnt >> 8) & 0xFF;
+    f_data[7] = (_rxFcnt >> 16) & 0xFF;
+    f_data[8] = (_rxFcnt >> 24) & 0xFF;
     f_data[9] = _events->RxPort;
 
     for(int i = 0; i < size; i++)
@@ -1136,7 +1590,6 @@
 
 
 std::string CommandTerminal::formatPacket(std::vector<uint8_t> payload, bool hex) {
-    uint32_t fcnt = _dot->getDownLinkCounter();
 
     if(_dot->getAckRequested()) {
         f_data[0] = 0;
@@ -1148,10 +1601,10 @@
     f_data[2] = (_rxAddress >> 8) & 0xFF;
     f_data[3] = (_rxAddress >> 16) & 0xFF;
     f_data[4] = (_rxAddress >> 24) & 0xFF;
-    f_data[5] = fcnt & 0xFF;
-    f_data[6] = (fcnt >> 8) & 0xFF;
-    f_data[7] = (fcnt >> 16) & 0xFF;
-    f_data[8] = (fcnt >> 24) & 0xFF;
+    f_data[5] = _rxFcnt & 0xFF;
+    f_data[6] = (_rxFcnt >> 8) & 0xFF;
+    f_data[7] = (_rxFcnt >> 16) & 0xFF;
+    f_data[8] = (_rxFcnt >> 24) & 0xFF;
     f_data[9] = _events->RxPort;
 
     for(size_t i = 0; i < payload.size(); i++)
@@ -1174,6 +1627,7 @@
 
         for (size_t i=0; i < payload.size(); i+=2) {
             if (sscanf((char*)&payload[i], "%2x", &temp) == 1) {
+                logDebug("Converted %s to %d", payload[i] + payload[i+1], temp);
                 converted.push_back((uint8_t)temp);
             }
         }