int srv_get_waypoints(LonLat32* waypoints, int path_len) {
/** request waypoints **/
Serial.println("request waypoints");
/**wait for waypoints **/
//initialize timer
unsigned long timer = millis();
int i = 0;
while(1) {
if (millis() - timer > 1000) {
//timeout
Serial.println("request timeout");
return -1;
}
if (Serial.available()) {
// check for acknowledgement
String check_srv = Serial.readStringUntil('\n');
if (check_srv == "server ready") {
String get_waypoint = Serial.readStringUntil('\n');
Serial.print("waypoint: ");
Serial.println(get_waypoint);
char first_letter = get_waypoint.charAt(0);
if (first_letter == 'E') {
break;
}
if (first_letter == 'W') {
/* save waypoint
* format/position:
* W XXXXXXX -XXXXXXXX<\n>
* 2 8 10 18
*
* lat: from 2-8, lon: from 10-18
*/
String lat_ = get_waypoint.substring(2,9);
String lon_ = get_waypoint.substring(10,19);
waypoints[i] = LonLat32(lon_.toInt(), lat_.toInt());
Serial.println("A");
Serial.println();
}
i++;
}
// restart timer for new request
timer = millis();
}
}
return 0;
}
int srv_get_waypoints(LonLat32* waypoints, int path_len)
{
// make sure println prints the last thing
Serial.println("");
Serial.println("A");
// store waypoint from server and acknowledge it
int i = 0;
unsigned long startTime = millis();
while (true)
{
if (millis() - startTime > 1000)
{
Serial.println("TIMEOUT");
return false;
}
if (Serial.available()) {
String inLine = Serial.readStringUntil('\n');
// session has ended
if (inLine.substring(0,1) == "E") {
break;
}
// waypoint string
if (inLine.substring(0,1) == "W") {
// store the waypoint
waypoints[i] = LonLat32(inLine.substring(10,19).toInt(),inLine.substring(2,9).toInt());
// send acknowledgement
Serial.println("A\n");
}
i += 1;
startTime = millis();
}
}
Serial.println(millis() - startTime);
return 0;
}
