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@79:f356009dbc12, 2013-12-27 (annotated)

Committer:
jengbrecht
Date:
Fri Dec 27 14:41:03 2013 +0000
Revision:
79:f356009dbc12
Parent:
76:371aab9902a4
Child:
93:aa7a48e65974
Added a few more features to Wifi.

Who changed what in which revision?

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