Multi-Hackers / SocketModem

A library for talking to Multi-Tech's Cellular SocketModem Devices.

Dependents:   M2X_dev axeda_wrapper_dev MTS_M2x_Example1 MTS_Cellular_Connect_Example ... more

wifi/Wifi.cpp@93:aa7a48e65974, 2013-12-27 (annotated)

Committer:
jengbrecht
Date:
Fri Dec 27 23:55:18 2013 +0000
Revision:
93:aa7a48e65974
Parent:
79:f356009dbc12
Child:
94:1baa587e89ae
Got connect, getSignalStrength, sendCommand, sendBasicCommand, isConnected and setCmdMode, all working in WiFi pretty well.  Still need a little more testing, but worthy of a commit. :-)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jengbrecht 69:f3e696bbb0d5 1 #include "Wifi.h"
jengbrecht 93:aa7a48e65974 2 #include <string>
jengbrecht 69:f3e696bbb0d5 3
jengbrecht 69:f3e696bbb0d5 4 Wifi* Wifi::instance = NULL;
jengbrecht 69:f3e696bbb0d5 5
jengbrecht 69:f3e696bbb0d5 6 Wifi* Wifi::getInstance()
jengbrecht 69:f3e696bbb0d5 7 {
jengbrecht 69:f3e696bbb0d5 8 if(instance == NULL) {
jengbrecht 69:f3e696bbb0d5 9 instance = new Wifi(NULL);
jengbrecht 69:f3e696bbb0d5 10 }
jengbrecht 69:f3e696bbb0d5 11 return instance;
jengbrecht 69:f3e696bbb0d5 12 }
jengbrecht 69:f3e696bbb0d5 13
jengbrecht 69:f3e696bbb0d5 14 bool Wifi::init(MTSBufferedIO* io)
jengbrecht 69:f3e696bbb0d5 15 {
jengbrecht 69:f3e696bbb0d5 16 if (io == NULL) {
jengbrecht 69:f3e696bbb0d5 17 return false;
jengbrecht 69:f3e696bbb0d5 18 }
jengbrecht 69:f3e696bbb0d5 19 instance->io = io;
jengbrecht 93:aa7a48e65974 20 if (setCmdMode(true)) {
jengbrecht 74:9f87bd22c222 21 return true;
jengbrecht 74:9f87bd22c222 22 }
jengbrecht 74:9f87bd22c222 23 return false;
jengbrecht 69:f3e696bbb0d5 24 }
jengbrecht 69:f3e696bbb0d5 25
jengbrecht 69:f3e696bbb0d5 26 Wifi::Wifi(MTSBufferedIO* io)
jengbrecht 69:f3e696bbb0d5 27 : io(io)
jengbrecht 69:f3e696bbb0d5 28 , wifiConnected(false)
jengbrecht 79:f356009dbc12 29 , _ssid("")
jengbrecht 69:f3e696bbb0d5 30 , mode(TCP)
jengbrecht 69:f3e696bbb0d5 31 , socketOpened(false)
jengbrecht 69:f3e696bbb0d5 32 , socketCloseable(true)
jengbrecht 69:f3e696bbb0d5 33 , local_port(0)
jengbrecht 69:f3e696bbb0d5 34 , host_port(0)
jengbrecht 79:f356009dbc12 35 , cmdOn(false)
jengbrecht 69:f3e696bbb0d5 36 {
jengbrecht 69:f3e696bbb0d5 37
jengbrecht 69:f3e696bbb0d5 38 }
jengbrecht 69:f3e696bbb0d5 39
jengbrecht 69:f3e696bbb0d5 40 Wifi::~Wifi()
jengbrecht 69:f3e696bbb0d5 41 {
jengbrecht 69:f3e696bbb0d5 42 }
jengbrecht 69:f3e696bbb0d5 43
jengbrecht 69:f3e696bbb0d5 44 bool Wifi::connect()
jengbrecht 69:f3e696bbb0d5 45 {
jengbrecht 69:f3e696bbb0d5 46 //Check if socket is open
jengbrecht 69:f3e696bbb0d5 47 if(socketOpened) {
jengbrecht 69:f3e696bbb0d5 48 return true;
jengbrecht 69:f3e696bbb0d5 49 }
jengbrecht 69:f3e696bbb0d5 50
jengbrecht 69:f3e696bbb0d5 51 //Run Test first to validate a good state
jengbrecht 69:f3e696bbb0d5 52 if(isConnected()) {
jengbrecht 69:f3e696bbb0d5 53 return true;
jengbrecht 69:f3e696bbb0d5 54 }
jengbrecht 69:f3e696bbb0d5 55
jengbrecht 93:aa7a48e65974 56 if (_ssid.size() == 0) {
jengbrecht 93:aa7a48e65974 57 printf("[ERROR] No SSID has been set\n\r");
jengbrecht 93:aa7a48e65974 58 return false;
jengbrecht 93:aa7a48e65974 59 }
jengbrecht 93:aa7a48e65974 60
jengbrecht 93:aa7a48e65974 61 if (!cmdOn) {
jengbrecht 93:aa7a48e65974 62 if(!setCmdMode(true)) {
jengbrecht 93:aa7a48e65974 63 printf("[ERROR] Failed to enter command mode\n\r");
jengbrecht 93:aa7a48e65974 64 return false;
jengbrecht 93:aa7a48e65974 65 }
jengbrecht 93:aa7a48e65974 66 }
jengbrecht 93:aa7a48e65974 67
jengbrecht 69:f3e696bbb0d5 68 //Check RSSI: AT+CSQ
jengbrecht 93:aa7a48e65974 69 int rssi = getSignalStrength();
jengbrecht 93:aa7a48e65974 70 printf("[DEBUG] Signal strength: %d\r\n", rssi);
jengbrecht 69:f3e696bbb0d5 71
jengbrecht 69:f3e696bbb0d5 72 //Possibly add a scan command here and look for the network....
jengbrecht 69:f3e696bbb0d5 73
jengbrecht 93:aa7a48e65974 74 printf("[DEBUG] Starting Network Configuration\n\r");
jengbrecht 69:f3e696bbb0d5 75 //Set device to manual infrastructure mode
sgodinez 73:bb5bbca971ae 76 if (sendBasicCommand("set wlan join 0", 1000) != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 77 return false;
jengbrecht 69:f3e696bbb0d5 78 }
jengbrecht 69:f3e696bbb0d5 79
jengbrecht 69:f3e696bbb0d5 80 //Set device to channel auto-scanning mode
sgodinez 73:bb5bbca971ae 81 if (sendBasicCommand("set wlan channel 0", 1000) != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 82 return false;
jengbrecht 69:f3e696bbb0d5 83 }
jengbrecht 69:f3e696bbb0d5 84
jengbrecht 69:f3e696bbb0d5 85 //Set device so no data is transmitted immediately following a socket connection
sgodinez 73:bb5bbca971ae 86 if (sendBasicCommand("set comm remote 0", 1000) != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 87 return false;
jengbrecht 69:f3e696bbb0d5 88 }
jengbrecht 69:f3e696bbb0d5 89
jengbrecht 69:f3e696bbb0d5 90 //Set device into DHCP mode
sgodinez 73:bb5bbca971ae 91 if (sendBasicCommand("set ip dhcp 1", 1000) != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 92 return false;
jengbrecht 69:f3e696bbb0d5 93 }
jengbrecht 69:f3e696bbb0d5 94
jengbrecht 69:f3e696bbb0d5 95 //join my_network
jengbrecht 69:f3e696bbb0d5 96 printf("[DEBUG] Making SSID Connection Attempt. SSID[%s]\r\n", _ssid.c_str());
jengbrecht 93:aa7a48e65974 97 std::string result = sendCommand("join " + _ssid, 15000, "Listen");
jengbrecht 93:aa7a48e65974 98 //printf("Connect Status: %s\n\r", result.c_str());
jengbrecht 69:f3e696bbb0d5 99
jengbrecht 93:aa7a48e65974 100 //Check whether connection was successful
jengbrecht 93:aa7a48e65974 101 if(result.find("Associated!") != string::npos) {
jengbrecht 93:aa7a48e65974 102 int start = result.find("IP=");
jengbrecht 93:aa7a48e65974 103 int stop = result.find(":", start);
jengbrecht 93:aa7a48e65974 104 local_address = result.substr(start + 3, stop - start - 3);
jengbrecht 93:aa7a48e65974 105 printf("[INFO] WiFi Connection Established: IP[%s]\r\n", local_address.c_str());
jengbrecht 93:aa7a48e65974 106 wifiConnected = true;
jengbrecht 93:aa7a48e65974 107 } else {
jengbrecht 93:aa7a48e65974 108 wifiConnected = false;
jengbrecht 93:aa7a48e65974 109 }
jengbrecht 69:f3e696bbb0d5 110
jengbrecht 69:f3e696bbb0d5 111 return wifiConnected;
jengbrecht 69:f3e696bbb0d5 112 }
jengbrecht 69:f3e696bbb0d5 113
jengbrecht 69:f3e696bbb0d5 114 void Wifi::disconnect()
jengbrecht 69:f3e696bbb0d5 115 {
jengbrecht 79:f356009dbc12 116 printf("[DEBUG] Disconnecting from network\r\n");
jengbrecht 79:f356009dbc12 117
jengbrecht 79:f356009dbc12 118 if(socketOpened) {
jengbrecht 79:f356009dbc12 119 close();
jengbrecht 79:f356009dbc12 120 }
jengbrecht 79:f356009dbc12 121
jengbrecht 79:f356009dbc12 122 bool success = false;
jengbrecht 79:f356009dbc12 123 if (setCmdMode(true)) {
jengbrecht 79:f356009dbc12 124 std::string response = sendCommand("leave", 10000);
jengbrecht 79:f356009dbc12 125 if (response.find("DeAuth") != string::npos) {
jengbrecht 79:f356009dbc12 126 success = true;
jengbrecht 79:f356009dbc12 127 }
jengbrecht 79:f356009dbc12 128 }
jengbrecht 79:f356009dbc12 129
jengbrecht 79:f356009dbc12 130 if(success) {
jengbrecht 79:f356009dbc12 131 printf("[DEBUG] Successfully disconnected from network\r\n");
jengbrecht 79:f356009dbc12 132 } else {
jengbrecht 79:f356009dbc12 133 printf("[ERROR] Failed in disconnecting from network. Continuing ...\r\n");
jengbrecht 79:f356009dbc12 134 }
jengbrecht 79:f356009dbc12 135
jengbrecht 79:f356009dbc12 136 wifiConnected = false;
jengbrecht 69:f3e696bbb0d5 137 }
jengbrecht 69:f3e696bbb0d5 138
jengbrecht 69:f3e696bbb0d5 139 bool Wifi::isConnected()
jengbrecht 69:f3e696bbb0d5 140 {
jengbrecht 79:f356009dbc12 141 //1) Check if SSID was set
jengbrecht 79:f356009dbc12 142 if(_ssid.size() == 0) {
jengbrecht 79:f356009dbc12 143 printf("[DEBUG] SSID is not set\r\n");
jengbrecht 79:f356009dbc12 144 return false;
jengbrecht 79:f356009dbc12 145 }
jengbrecht 79:f356009dbc12 146
jengbrecht 79:f356009dbc12 147 //1) Check that we do not have a live connection up
jengbrecht 79:f356009dbc12 148 if(socketOpened) {
jengbrecht 79:f356009dbc12 149 printf("[DEBUG] Socket is opened\r\n");
jengbrecht 79:f356009dbc12 150 return true;
jengbrecht 79:f356009dbc12 151 }
jengbrecht 79:f356009dbc12 152
jengbrecht 79:f356009dbc12 153 //2) Query the wifi module
jengbrecht 79:f356009dbc12 154 wifiConnected = false;
jengbrecht 93:aa7a48e65974 155 std::string result = sendCommand("show net", 5000, "Links");
jengbrecht 93:aa7a48e65974 156 //printf("netResult: %s\n\r", result);
jengbrecht 93:aa7a48e65974 157 if(result.find("Assoc=OK") != std::string::npos) {
jengbrecht 79:f356009dbc12 158 wifiConnected = true;
jengbrecht 79:f356009dbc12 159 }
jengbrecht 79:f356009dbc12 160
jengbrecht 79:f356009dbc12 161 return wifiConnected;
jengbrecht 69:f3e696bbb0d5 162 }
jengbrecht 69:f3e696bbb0d5 163
jengbrecht 69:f3e696bbb0d5 164 bool Wifi::bind(unsigned int port)
jengbrecht 69:f3e696bbb0d5 165 {
jengbrecht 69:f3e696bbb0d5 166 return true;
jengbrecht 69:f3e696bbb0d5 167 }
jengbrecht 69:f3e696bbb0d5 168
jengbrecht 69:f3e696bbb0d5 169 bool Wifi::open(const std::string& address, unsigned int port, Mode mode)
jengbrecht 69:f3e696bbb0d5 170 {
jengbrecht 69:f3e696bbb0d5 171 //set comm size??? are advanced Socket settings
jengbrecht 69:f3e696bbb0d5 172 //set comm time??? are advanced Socket settings
jengbrecht 69:f3e696bbb0d5 173 return true;
jengbrecht 69:f3e696bbb0d5 174 }
jengbrecht 69:f3e696bbb0d5 175
jengbrecht 69:f3e696bbb0d5 176 bool Wifi::isOpen()
jengbrecht 69:f3e696bbb0d5 177 {
jengbrecht 69:f3e696bbb0d5 178 return true;
jengbrecht 69:f3e696bbb0d5 179 }
jengbrecht 69:f3e696bbb0d5 180
jengbrecht 69:f3e696bbb0d5 181 bool Wifi::close()
jengbrecht 69:f3e696bbb0d5 182 {
jengbrecht 69:f3e696bbb0d5 183 return true;
jengbrecht 69:f3e696bbb0d5 184 }
jengbrecht 69:f3e696bbb0d5 185
jengbrecht 69:f3e696bbb0d5 186 int Wifi::read(char* data, int max, int timeout)
jengbrecht 69:f3e696bbb0d5 187 {
jengbrecht 69:f3e696bbb0d5 188 return 0;
jengbrecht 69:f3e696bbb0d5 189 }
jengbrecht 69:f3e696bbb0d5 190
jengbrecht 69:f3e696bbb0d5 191 int Wifi::write(const char* data, int length, int timeout)
jengbrecht 69:f3e696bbb0d5 192 {
jengbrecht 69:f3e696bbb0d5 193 return 0;
jengbrecht 69:f3e696bbb0d5 194 }
jengbrecht 69:f3e696bbb0d5 195
jengbrecht 69:f3e696bbb0d5 196 unsigned int Wifi::readable()
jengbrecht 69:f3e696bbb0d5 197 {
jengbrecht 69:f3e696bbb0d5 198 if(io == NULL) {
jengbrecht 69:f3e696bbb0d5 199 printf("[ERROR] MTSBufferedIO not set\r\n");
jengbrecht 69:f3e696bbb0d5 200 return 0;
jengbrecht 69:f3e696bbb0d5 201 }
jengbrecht 69:f3e696bbb0d5 202 if(!socketOpened) {
jengbrecht 69:f3e696bbb0d5 203 printf("[ERROR] Socket is not open\r\n");
jengbrecht 69:f3e696bbb0d5 204 return 0;
jengbrecht 69:f3e696bbb0d5 205 }
jengbrecht 69:f3e696bbb0d5 206 return io->readable();
jengbrecht 69:f3e696bbb0d5 207 }
jengbrecht 69:f3e696bbb0d5 208
jengbrecht 69:f3e696bbb0d5 209 unsigned int Wifi::writeable()
jengbrecht 69:f3e696bbb0d5 210 {
jengbrecht 69:f3e696bbb0d5 211 if(io == NULL) {
jengbrecht 69:f3e696bbb0d5 212 printf("[ERROR] MTSBufferedIO not set\r\n");
jengbrecht 69:f3e696bbb0d5 213 return 0;
jengbrecht 69:f3e696bbb0d5 214 }
jengbrecht 69:f3e696bbb0d5 215 if(!socketOpened) {
jengbrecht 69:f3e696bbb0d5 216 printf("[ERROR] Socket is not open\r\n");
jengbrecht 69:f3e696bbb0d5 217 return 0;
jengbrecht 69:f3e696bbb0d5 218 }
jengbrecht 69:f3e696bbb0d5 219
jengbrecht 69:f3e696bbb0d5 220 return io->writeable();
jengbrecht 69:f3e696bbb0d5 221 }
jengbrecht 69:f3e696bbb0d5 222
jengbrecht 69:f3e696bbb0d5 223 void Wifi::reset()
jengbrecht 69:f3e696bbb0d5 224 {
jengbrecht 69:f3e696bbb0d5 225 }
jengbrecht 69:f3e696bbb0d5 226
sgodinez 73:bb5bbca971ae 227 Code Wifi::setNetwork(const std::string& ssid, const std::string& key, SecurityType type)
jengbrecht 69:f3e696bbb0d5 228 {
sgodinez 73:bb5bbca971ae 229 Code code;
jengbrecht 69:f3e696bbb0d5 230
jengbrecht 69:f3e696bbb0d5 231 //Set the appropraite SSID
jengbrecht 69:f3e696bbb0d5 232 code = sendBasicCommand("set wlan ssid " + ssid, 1000);
sgodinez 73:bb5bbca971ae 233 if (code != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 234 return code;
jengbrecht 69:f3e696bbb0d5 235 }
jengbrecht 69:f3e696bbb0d5 236
jengbrecht 69:f3e696bbb0d5 237 //Set the security key
jengbrecht 69:f3e696bbb0d5 238 if (type == WEP64 || type == WEP128) {
jengbrecht 69:f3e696bbb0d5 239 //Set the WEP key if using WEP encryption
jengbrecht 69:f3e696bbb0d5 240 code = sendBasicCommand("set wlan key " + key, 1000);
sgodinez 73:bb5bbca971ae 241 if (code != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 242 return code;
jengbrecht 69:f3e696bbb0d5 243 }
jengbrecht 69:f3e696bbb0d5 244 } else if (type == WPA || type == WPA2) {
jengbrecht 69:f3e696bbb0d5 245 //Set the WPA key if using WPA encryption
jengbrecht 69:f3e696bbb0d5 246 code = sendBasicCommand("set wlan phrase " + key, 1000);
sgodinez 73:bb5bbca971ae 247 if (code != SUCCESS) {
jengbrecht 69:f3e696bbb0d5 248 return code;
jengbrecht 69:f3e696bbb0d5 249 }
jengbrecht 69:f3e696bbb0d5 250 }
jengbrecht 69:f3e696bbb0d5 251
jengbrecht 69:f3e696bbb0d5 252 _ssid = ssid;
sgodinez 73:bb5bbca971ae 253 return SUCCESS;
jengbrecht 69:f3e696bbb0d5 254 }
jengbrecht 69:f3e696bbb0d5 255
jengbrecht 69:f3e696bbb0d5 256 int Wifi::getSignalStrength()
jengbrecht 69:f3e696bbb0d5 257 {
jengbrecht 93:aa7a48e65974 258 string response = sendCommand("show rssi", 2000, "dBm");
jengbrecht 74:9f87bd22c222 259 if (response.find("RSSI") == string::npos) {
jengbrecht 74:9f87bd22c222 260 return -1;
jengbrecht 74:9f87bd22c222 261 }
jengbrecht 74:9f87bd22c222 262 int start = response.find('(');
jengbrecht 74:9f87bd22c222 263 int stop = response.find(')', start);
jengbrecht 74:9f87bd22c222 264 string signal = response.substr(start + 1, stop - start - 1);
jengbrecht 74:9f87bd22c222 265 int value;
jengbrecht 74:9f87bd22c222 266 sscanf(signal.c_str(), "%d", &value);
jengbrecht 74:9f87bd22c222 267 return value;
jengbrecht 69:f3e696bbb0d5 268 }
jengbrecht 69:f3e696bbb0d5 269
jengbrecht 79:f356009dbc12 270 bool Wifi::setCmdMode(bool on)
jengbrecht 74:9f87bd22c222 271 {
jengbrecht 79:f356009dbc12 272 if (on) {
jengbrecht 79:f356009dbc12 273 if (cmdOn) {
jengbrecht 79:f356009dbc12 274 return true;
jengbrecht 79:f356009dbc12 275 }
jengbrecht 79:f356009dbc12 276 wait(.5);
jengbrecht 93:aa7a48e65974 277 std::string response = sendCommand("$$", 2000, "CMD", '$');
jengbrecht 79:f356009dbc12 278 if (response.find("CMD") != string::npos) {
jengbrecht 79:f356009dbc12 279 cmdOn = true;
jengbrecht 79:f356009dbc12 280 return true;
jengbrecht 79:f356009dbc12 281 }
jengbrecht 93:aa7a48e65974 282 printf("Failed Response: %s\n\r", response.c_str());
jengbrecht 79:f356009dbc12 283 return false;
jengbrecht 79:f356009dbc12 284 } else {
jengbrecht 79:f356009dbc12 285 if (!cmdOn) {
jengbrecht 79:f356009dbc12 286 return true;
jengbrecht 79:f356009dbc12 287 }
jengbrecht 93:aa7a48e65974 288 std::string response = sendCommand("exit", 2000, "EXIT");
jengbrecht 79:f356009dbc12 289 if (response.find("EXIT") != string::npos) {
jengbrecht 79:f356009dbc12 290 cmdOn = false;
jengbrecht 79:f356009dbc12 291 return true;
jengbrecht 79:f356009dbc12 292 }
jengbrecht 79:f356009dbc12 293 return false;
jengbrecht 74:9f87bd22c222 294 }
jengbrecht 74:9f87bd22c222 295 }
jengbrecht 69:f3e696bbb0d5 296
sgodinez 73:bb5bbca971ae 297 Code Wifi::sendBasicCommand(string command, int timeoutMillis, char esc)
jengbrecht 69:f3e696bbb0d5 298 {
jengbrecht 69:f3e696bbb0d5 299 if(socketOpened) {
jengbrecht 69:f3e696bbb0d5 300 printf("[ERROR] socket is open. Can not send AT commands\r\n");
sgodinez 73:bb5bbca971ae 301 return ERROR;
jengbrecht 69:f3e696bbb0d5 302 }
jengbrecht 69:f3e696bbb0d5 303
jengbrecht 93:aa7a48e65974 304 string response = sendCommand(command, timeoutMillis, "AOK", esc);
jengbrecht 69:f3e696bbb0d5 305 //printf("Response: %s\n\r", response.c_str());
jengbrecht 69:f3e696bbb0d5 306 if (response.size() == 0) {
sgodinez 73:bb5bbca971ae 307 return NO_RESPONSE;
jengbrecht 69:f3e696bbb0d5 308 } else if (response.find("AOK") != string::npos) {
sgodinez 73:bb5bbca971ae 309 return SUCCESS;
jengbrecht 69:f3e696bbb0d5 310 } else if (response.find("ERR") != string::npos) {
sgodinez 73:bb5bbca971ae 311 return ERROR;
jengbrecht 69:f3e696bbb0d5 312 } else {
sgodinez 73:bb5bbca971ae 313 return FAILURE;
jengbrecht 69:f3e696bbb0d5 314 }
jengbrecht 69:f3e696bbb0d5 315 }
jengbrecht 69:f3e696bbb0d5 316
jengbrecht 93:aa7a48e65974 317 string Wifi::sendCommand(string command, int timeoutMillis, std::string response, char esc)
jengbrecht 69:f3e696bbb0d5 318 {
jengbrecht 69:f3e696bbb0d5 319 if(io == NULL) {
jengbrecht 69:f3e696bbb0d5 320 printf("[ERROR] MTSBufferedIO not set\r\n");
jengbrecht 69:f3e696bbb0d5 321 return "";
jengbrecht 69:f3e696bbb0d5 322 }
jengbrecht 93:aa7a48e65974 323 if(socketOpened && command.compare("$$") != 0) {
jengbrecht 79:f356009dbc12 324 printf("[ERROR] socket is open. Can not send AT commands\r\n");
jengbrecht 69:f3e696bbb0d5 325 return "";
jengbrecht 69:f3e696bbb0d5 326 }
jengbrecht 69:f3e696bbb0d5 327
jengbrecht 69:f3e696bbb0d5 328 io->rxClear();
jengbrecht 69:f3e696bbb0d5 329 io->txClear();
jengbrecht 69:f3e696bbb0d5 330 std::string result;
jengbrecht 79:f356009dbc12 331
sgodinez 73:bb5bbca971ae 332 //Attempt to write command
sgodinez 73:bb5bbca971ae 333 if(io->write(command.data(), command.size(), timeoutMillis) != command.size()) {
sgodinez 73:bb5bbca971ae 334 //Failed to write command
sgodinez 73:bb5bbca971ae 335 printf("[ERROR] failed to send command to radio within %d milliseconds\r\n", timeoutMillis);
sgodinez 73:bb5bbca971ae 336 return "";
sgodinez 73:bb5bbca971ae 337 }
jengbrecht 79:f356009dbc12 338
sgodinez 73:bb5bbca971ae 339 //Send Escape Character
sgodinez 73:bb5bbca971ae 340 if (esc != 0x00) {
sgodinez 73:bb5bbca971ae 341 if(io->write(esc, timeoutMillis) != 1) {
sgodinez 73:bb5bbca971ae 342 printf("[ERROR] failed to send '%c' to radio within %d milliseconds\r\n", esc, timeoutMillis);
sgodinez 73:bb5bbca971ae 343 return "";
sgodinez 73:bb5bbca971ae 344 }
sgodinez 73:bb5bbca971ae 345 }
sgodinez 73:bb5bbca971ae 346
jengbrecht 69:f3e696bbb0d5 347 int timer = 0;
sgodinez 73:bb5bbca971ae 348 size_t previous = 0;
jengbrecht 69:f3e696bbb0d5 349 char tmp[256];
jengbrecht 69:f3e696bbb0d5 350 tmp[255] = 0;
jengbrecht 69:f3e696bbb0d5 351 bool done = false;
jengbrecht 69:f3e696bbb0d5 352 do {
jengbrecht 93:aa7a48e65974 353 wait(.2);
jengbrecht 93:aa7a48e65974 354 timer = timer + 200;
sgodinez 73:bb5bbca971ae 355 previous = result.size();
jengbrecht 93:aa7a48e65974 356 int size = io->read(tmp, 255, 0); //1 less than allocated
jengbrecht 69:f3e696bbb0d5 357 if(size > 0) {
jengbrecht 69:f3e696bbb0d5 358 result.append(tmp, size);
jengbrecht 93:aa7a48e65974 359 if (response.size() != 0) {
jengbrecht 93:aa7a48e65974 360 if (result.find(response) != string::npos) {
jengbrecht 93:aa7a48e65974 361 return result;
jengbrecht 93:aa7a48e65974 362 }
jengbrecht 93:aa7a48e65974 363 } else {
jengbrecht 93:aa7a48e65974 364 done = (result.size() == previous);
jengbrecht 69:f3e696bbb0d5 365 }
jengbrecht 69:f3e696bbb0d5 366 }
jengbrecht 69:f3e696bbb0d5 367 if(timer >= timeoutMillis) {
jengbrecht 93:aa7a48e65974 368 printf("[WARNING] sendCommand [%s] timed out after %d milliseconds\r\n", command.c_str(), timeoutMillis);
jengbrecht 69:f3e696bbb0d5 369 done = true;
jengbrecht 69:f3e696bbb0d5 370 }
jengbrecht 69:f3e696bbb0d5 371 } while (!done);
jengbrecht 79:f356009dbc12 372
jengbrecht 93:aa7a48e65974 373 //printf("Result: %s\n\r", result.c_str());
jengbrecht 69:f3e696bbb0d5 374 return result;
jengbrecht 69:f3e696bbb0d5 375 }