fix modem

Former-commit-id: e73d127

Author: maidnl

libraries/WiFiS3/src/Modem.cpp

@@ -30,20 +30,18 @@ void ModemClass::end(){
 }
 
 bool ModemClass::write(const string &prompt, string &data_res, char * fmt, ...){
+  
+  data_res.clear();
+
   memset(tx_buff,0x00,MAX_BUFF_SIZE);
   va_list va;
   va_start (va, fmt);
   vsprintf ((char *)tx_buff, fmt, va);
   va_end (va);
   #ifdef MODEM_DEBUG
-    Serial.print("tx_buff: ");
-    for(int i =0; i<MAX_BUFF_SIZE; i++) {
-      Serial.print(tx_buff[i], HEX);
-      Serial.print(" ");
-    }
-    Serial.println();
+    Serial.print("Write Call, command sent: ");
     Serial.write(tx_buff,strlen((char *)tx_buff));
-    Serial.println("ciao");
+    Serial.println();
   #endif
   _serial->write(tx_buff,strlen((char *)tx_buff));
   return buf_read(prompt,data_res);;
@@ -52,20 +50,24 @@ bool ModemClass::write(const string &prompt, string &data_res, char * fmt, ...){
 
 bool ModemClass::buf_read(const string &prompt, string &data_res) {
    bool res = false;
+   bool found = false;
    unsigned long start_time = millis();
-   while(millis() - start_time < _timeout){
+   while(millis() - start_time < _timeout && !found){
       while(_serial->available()){
          char c = _serial->read();
          data_res += c;
-         Serial.print(c);
+         //Serial.print(c);
          if(string::npos != data_res.rfind(PROMPT_OK)){
-            data_res.substr(0, data_res.length() - sizeof(PROMPT_OK));
+            found = true;
+            data_res = data_res.substr(0, data_res.length() - sizeof(PROMPT_OK));
             if(prompt != DO_NOT_CHECK_CMD) {
                if(removeAtBegin(data_res, prompt)) {
                   res = true;
                }
             }
-            res = true;
+            else {
+              res = true;
+            }
             break;
          } 
          else if (string::npos != data_res.rfind(PROMPT_ERROR)) {
@@ -75,10 +77,19 @@ bool ModemClass::buf_read(const string &prompt, string &data_res) {
          }
       }
     }
+    trim(data_res);
     #ifdef MODEM_DEBUG
-      Serial.print("data_res: ");
-      Serial.println(data_res.c_str());
+      Serial.print("Write Call, response rx |>>");
+      Serial.print(data_res.c_str());
+      Serial.println("<<|");
+      if(res) {
+         Serial.println("Result: OK");
+      }
+      else {
+         Serial.println("Result: FAILED");
+      }
     #endif
+      
    return res;
 }
 

libraries/WiFiS3/src/WiFi.cpp

@@ -23,12 +23,26 @@ int CWifi::begin(const char* ssid) {
 /* -------------------------------------------------------------------------- */
 int CWifi::begin(const char* ssid, const char *passphrase) {
 /* -------------------------------------------------------------------------- */
-   modem.begin();
    string res = "";
-   if(modem.write(string(PROMPT(_BEGINSTA)),res, "%s%s,%s\r\n" , CMD_WRITE(_BEGINSTA), ssid, passphrase)) {
-      return atoi(res.c_str());
+   modem.begin();
+   if(!modem.write(string(PROMPT(_MODE)),res, "%s%d\r\n" , CMD_WRITE(_MODE), 1)) {
+      return WL_CONNECT_FAILED;
    }
-  return 0;
+
+   if(!modem.write(string(PROMPT(_BEGINSTA)),res, "%s%s,%s\r\n" , CMD_WRITE(_BEGINSTA), ssid, passphrase)) {
+      return WL_CONNECT_FAILED;
+   }
+
+   unsigned long start_time = millis();
+   while(millis() - start_time < 10000){
+      if(modem.write(string(PROMPT(_GETSTATUS)),res,CMD(_GETSTATUS))) {
+         if(atoi(res.c_str()) == WL_CONNECTED) {
+            return WL_CONNECTED;
+         }
+      }      
+   }
+
+  return WL_CONNECT_FAILED;
 }
 
 /* passphrase is needed so a default one will be set */