PowerUp sequence tests

module-cellular/Modem/Modem.cpp

@@ -10,3 +10,91 @@
 
 
 #include "Modem.hpp"
+
+#include <cstring>
+
+#include "log/log.hpp"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+Modem::Modem() {
+    cellular = bsp::Cellular::Create();
+}
+
+Modem::~Modem() {
+
+}
+
+
+void Modem::Init() {
+
+    // At first send AT command to check if modem is turned on
+    if(SendAT("AT\r\n",500) == -1){
+        // If no response, power up modem and try again
+        cellular->PowerUp();
+
+        uint32_t retries = 10;
+        while(--retries){
+            if(SendAT("AT\r\n",500) == 0){
+                return;
+            }
+        }
+
+        if(retries == 0){
+            LOG_FATAL("No communication with GSM modem");
+            return;
+        }
+    }
+
+}
+
+
+int Modem::SendAT(const char *cmd, uint32_t timeout) {
+
+    char buff[256] = {0};
+
+    cellular->Write(const_cast<char*>(cmd),strlen(cmd));
+
+    if(cellular->Wait(500)){
+
+        vTaskDelay(500);
+        auto bytesRead = cellular->Read(buff,sizeof buff);
+
+        if (memstr((char *) buff, bytesRead, "OK"))
+        {
+            LOG_DEBUG("Received OK");
+            return 0;
+        }
+        else if (memstr((char *) buff, bytesRead, "ERROR"))
+        {
+            LOG_DEBUG("Received ERROR");
+            return -1;
+        }
+        else{
+            LOG_DEBUG("Received unknonwn response");
+            return -1;
+        }
+    }
+    else{
+        return -1;
+    }
+}
+
+int Modem::memstr(const char *haystack, int length, const char *needle) {
+
+    int i;
+    int j = 0;
+    if (needle[0] == '\0')
+        return 1;
+    for (i = 0; i < length; i++)
+        if (needle[j] == haystack[i])
+        {
+            j++;
+            if (needle[j] == '\0') // Entire needle was found
+                return 1;
+        }
+        else
+            j = 0;
+    return 0;
+}