Regenerating PPM signal based on distances from ultrasonic sensors, ESP8266 for connectin via wifi. Autonomous quadcopter behaviour, autonomou height holding. Flying direction based on front and back ultrasonic sensors.
Dependencies: ConfigFile HCSR04 PID PPM2 mbed-rtos mbed
Server.h
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #include "hardware.h" 00004 00005 00006 RawSerial esp(p13, p14); // tx, rx 00007 DigitalOut reset(p11); 00008 DigitalOut chd_dp(p10); 00009 00010 // Standard Mbed LED definitions 00011 DigitalOut led1(LED1); // (PTB18) 00012 DigitalOut led3(LED3); // (PTD1) 00013 00014 Timer t1; 00015 Timer t2; 00016 00017 char cmdbuff[32]; 00018 char replybuff[1024]; 00019 char webcount[8]; 00020 char webdata[2048]; // This may need to be bigger depending on WEB browser used 00021 char webbuff[8192]; // Currently using 1986 characters, Increase this if more web page data added 00022 char type[16]; 00023 char channel[2]; 00024 char ip_address[20]; 00025 int bufflen, DataRX, count, getcount, replycount, servreq, timeout; 00026 int bufl, ipdLen, linkID, weberror, webcounter; 00027 00028 void getreply(); void SendCMD(); 00029 void startserver(); 00030 void ReadWebData(); 00031 void sendpage(); 00032 void sendcheck(); 00033 void get_ip_address(char web_data[1024]); 00034 void SendWEB(); 00035 00036 void serverRun(); 00037 void serverMain(); 00038 void serverLoop(); 00039 void callback(); 00040 00041 int port = 80; // set server port 00042 int SERVtimeout = 8; // set server timeout in seconds in case link breaks. 00043 00044 00045 00046 //serverThread.start(serverRun); 00047 00048 void callback(){ 00049 ////pc.printf("callback!!!!!!!!!!!!!!!!_________________________\n\r"); 00050 led3 = 1; 00051 while (esp.readable()) { 00052 webbuff[count] = esp.getc(); 00053 count++; 00054 } 00055 if(strlen(webbuff)>bufflen) { 00056 //printf("bufflen %d \n\r"); 00057 DataRX=1; 00058 led3=0; 00059 } 00060 } 00061 00062 00063 void serverRun(){ 00064 chd_dp = 1; 00065 reset=0; 00066 00067 //pc.printf("\f\n\r------------ ESP8266 Hardware Reset --------------\n\r"); 00068 wait(0.5); 00069 reset=1; 00070 timeout=6000; 00071 getcount=500; 00072 getreply(); 00073 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F 00074 startserver(); 00075 webcounter = 0; 00076 Timer timer; 00077 bool pageWasSent = false; 00078 while(1){ 00079 if(DataRX==1) { 00080 timer.start(); 00081 ReadWebData(); 00082 // Save data to configFile 00083 if ((servreq == 1 && weberror == 0) && pageWasSent == false) { 00084 // send HTTP Response 00085 sendpage(); 00086 pageWasSent = true; 00087 timer.reset(); 00088 00089 } 00090 else{ 00091 pc.printf("Page was not sent \n\r"); 00092 } 00093 esp.attach(&callback); 00094 ////pc.printf(" IPD Data:\r\n\n Link ID = %d,\r\n IPD Header Length = %d \r\n IPD Type = %s\r\n", linkID, ipdLen, type); 00095 ////pc.printf("HTTP Packet: \n\n%s", webdata); 00096 ////pc.printf("Web Characters sent : %d\n\n", bufl); 00097 ////pc.printf("-------------------------------------\n\n"); 00098 servreq=0; 00099 memset(_serverMessage, '\0', sizeof(_serverMessage)); 00100 } 00101 if(timer.read_ms() > 3000){ 00102 pageWasSent = false; 00103 ////pc.printf("starting reading new configuration in server task\n\r"); 00104 // when stop sending answers 00105 timer.stop(); 00106 timer.reset(); 00107 if(_newP == _P && _newI == _I && _newD == _D){ 00108 _onlyDistanChanged = true; 00109 } 00110 _groundSetPoint = _newGroundSetPoint; 00111 _P = _newP; 00112 _I = _newI; 00113 _D = _newD; 00114 // Save data to configFile 00115 writeSettingsToConfig(); 00116 // notification that config of pid was changed 00117 _configChanges = true; 00118 // start or stop ground regulation 00119 //_groundRegulation = _tempGroundRegulation; 00120 if(_groundRegulation != _tempGroundRegulation){ 00121 _groundRegulation = _tempGroundRegulation; 00122 _groundDistance->resetError(); 00123 } 00124 00125 } 00126 00127 Thread::wait(100); 00128 } 00129 } 00130 00131 00132 // Static WEB page 00133 void sendpage(){ 00134 strcpy(webbuff, "<!DOCTYPE html>"); 00135 strcat(webbuff, "<html><head><title>ESP8266 Mbed LPC1768</title></head>"); 00136 strcat(webbuff, "<body>"); 00137 //strcat(webbuff, "<div style=\"text-align:center; background-color:#F4F4F4; color:#00AEDB;\"><h1>ESP8266 Mbed IoT Web PID Controller</h1>"); 00138 //strcat(webbuff, "Hit Count - "); 00139 //strcat(webbuff, webcount); 00140 //strcat(webbuff, "</div><br /><hr>"); 00141 //server message (mostly errors) 00142 //strcat(webbuff, "<div stye=\"text-align:center; background-color:#F4F4F4; color:#00AEDB;\"><h3>Server messages</h3>"); 00143 //strcat(webbuff, _serverMessage); 00144 //strcat(webbuff, "</div><br><hr>"); 00145 strcat(webbuff, "<form method=\"POST\">"); 00146 //if(led1==0) { 00147 // strcat(webbuff, "<p><input type=\"radio\" name=\"led1\" value=\"0\" checked> LED 1 off"); 00148 // strcat(webbuff, "<br><input type=\"radio\" name=\"led1\" value=\"1\" > LED 1 on<br>"); 00149 //} else { 00150 // strcat(webbuff, "<p><input type=\"radio\" name=\"led1\" value=\"0\" > LED 1 off"); 00151 // strcat(webbuff, "<br><input type=\"radio\" name=\"led1\" value=\"1\" checked> LED 1 on<br>"); 00152 //} 00153 // pid ground min output 00154 strcat(webbuff, "<table><tr>"); 00155 strcat(webbuff, "<td>PID min: </td> <td><input type=\"text\" name=\"groundPidMinOutput\" size=\"4\" value=\""); 00156 ConvertToCharArray(_groundPidMinOutput); 00157 strcat(webbuff, _str); 00158 strcat(webbuff, "\">"); 00159 strcat(webbuff, "</td></tr>"); 00160 strcat(webbuff, "<tr>"); 00161 //pid ground max output 00162 strcat(webbuff, "<td>PID max: </td><td> <input type=\"text\" name=\"groundPidMaxOutput\" size=\"4\" value=\""); 00163 ConvertToCharArray(_groundPidMaxOutput); 00164 strcat(webbuff, _str); 00165 strcat(webbuff, "\">"); 00166 strcat(webbuff, "</td></tr>"); 00167 //ground regulation bias 00168 strcat(webbuff, "<tr>"); 00169 strcat(webbuff, "<td>Bias: </td><td><input type=\"text\" name=\"bias\" size=\"4\" value=\""); 00170 ConvertToCharArray(_bias); 00171 strcat(webbuff, _str); 00172 strcat(webbuff, "\">"); 00173 strcat(webbuff, "</td></tr>"); 00174 strcat(webbuff, "<tr>"); 00175 strcat(webbuff, "<td>P:</td><td> <input type=\"text\" name=\"proportional\" size=\"4\" value=\""); 00176 ConvertToCharArray(_newP); 00177 strcat(webbuff, _str); 00178 strcat(webbuff, "\"></td></tr>"); 00179 strcat(webbuff, "<tr>"); 00180 strcat(webbuff, "<td>I: </td><td><input type=\"text\" name=\"integral\" size=\"4\" value=\""); 00181 ConvertToCharArray(_newI); 00182 strcat(webbuff, _str); 00183 strcat(webbuff, "\"></td></tr>"); 00184 strcat(webbuff, "<tr>"); 00185 strcat(webbuff, "<td>D: </td><td><input type=\"text\" name=\"derivative\" size=\"4\" value=\""); 00186 ConvertToCharArray(_newD); 00187 strcat(webbuff, _str); 00188 strcat(webbuff, "\"></td></tr>"); 00189 strcat(webbuff, "</table><br>"); 00190 //Ground set Point 00191 strcat(webbuff, "Regulovana vyska: <input type=\"text\" name=\"groundSetPoint\" size=\"4\" value=\""); 00192 ConvertToCharArray(_newGroundSetPoint); 00193 strcat(webbuff, _str); 00194 strcat(webbuff, "\"><br>"); 00195 // ground regulation 00196 if(_tempGroundRegulation) { 00197 strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" checked> Regulace vysky - zapnout"); 00198 strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" > Regulace vysky - vypnout</p>"); 00199 } else { 00200 strcat(webbuff, "<p><input type=\"radio\" name=\"groundRegulation\" value=\"1\" > Regulace vysky - zapnout"); 00201 strcat(webbuff, "<br><input type=\"radio\" name=\"groundRegulation\" value=\"0\" checked> Regulace vysky - vypnout<p>"); 00202 } 00203 if(_goAhead == false) { 00204 strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" > Autonomni pohyb - zapnout"); 00205 strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\" checked> Autonomni pohyb - vypnout</p>"); 00206 } else { 00207 strcat(webbuff, "<p><input type=\"radio\" name=\"goAhead\" value=\"1\" checked> Autonomni pohyb - zapnout"); 00208 strcat(webbuff, "<br><input type=\"radio\" name=\"goAhead\" value=\"0\"> Autonomni pohyb - vypnout</p>"); 00209 } 00210 strcat(webbuff, "<p><input type=\"radio\" name=\"nothing\" value=\"1\" style=\"display:none\" checked></p>"); 00211 strcat(webbuff, "<p><input type=\"submit\"></p>"); 00212 strcat(webbuff, "</form>"); 00213 strcat(webbuff, "</body></html>"); 00214 // end of WEB page data 00215 bufl = strlen(webbuff); // get total page buffer length 00216 sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length. 00217 pc.printf("%s \n\r", cmdbuff); 00218 timeout=200; 00219 getcount=7; 00220 SendCMD(); 00221 Thread::wait(1000); 00222 getreply(); 00223 pc.printf("%s \n\r", replybuff); 00224 SendWEB(); // send web page 00225 memset(webbuff, '\0', sizeof(webbuff)); 00226 sendcheck(); 00227 00228 } 00229 00230 void sendcheck(){ 00231 weberror=1; 00232 timeout= 1000; 00233 getcount = 50; 00234 t2.reset(); 00235 t2.start(); 00236 while(weberror==1 && t2.read() <5) { 00237 getreply(); 00238 if (strstr(replybuff, "SEND OK") != NULL) { 00239 weberror=0; // wait for valid SEND OK 00240 } 00241 } 00242 if(weberror==1) { // restart connection 00243 pc.printf("sendcheck - restarting connection \n\r"); 00244 strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); 00245 timeout=500; 00246 getcount=10; 00247 SendCMD(); 00248 getreply(); 00249 //pc.printf(replybuff); 00250 sprintf(cmdbuff,"AT+CIPSERVER=0,%d\r\n", port); 00251 timeout=500; 00252 getcount=10; 00253 SendCMD(); 00254 getreply(); 00255 //pc.printf(replybuff); 00256 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port); 00257 timeout=500; 00258 getcount=10; 00259 SendCMD(); 00260 getreply(); 00261 //pc.printf(replybuff); 00262 } else { 00263 //pc.printf("sendcheck ok \n\r"); 00264 sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection 00265 SendCMD(); 00266 getreply(); 00267 //pc.printf(replybuff); 00268 } 00269 t2.reset(); 00270 } 00271 00272 // Large WEB buffer data send 00273 void SendWEB() 00274 { 00275 int i=0; 00276 if(esp.writeable()) { 00277 while(webbuff[i]!='\0') { 00278 esp.putc(webbuff[i]); 00279 i++; 00280 } 00281 } 00282 } 00283 00284 // Reads and processes GET and POST web data 00285 void ReadWebData(){ 00286 wait_ms(200); 00287 esp.attach(NULL); 00288 count=0; 00289 DataRX=0; 00290 weberror=0; 00291 memset(webdata, '\0', sizeof(webdata)); 00292 int x = strcspn (webbuff,"+"); 00293 if(x) { 00294 strcpy(webdata, webbuff + x); 00295 //////pc.printf("webdata received: %s", webdata); 00296 weberror=0; 00297 int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type); 00298 if (strstr(webdata, "led1=1") != NULL ) { 00299 led1=1; 00300 } 00301 if (strstr(webdata, "led1=0") != NULL ) { 00302 led1=0; 00303 } 00304 //ground regulation 00305 if (strstr(webdata, "groundRegulation=1") != NULL ) 00306 _tempGroundRegulation = true; 00307 if (strstr(webdata, "groundRegulation=0") != NULL ) 00308 _tempGroundRegulation = false; 00309 // pid min Output 00310 if (strstr(webdata, "groundPidMinOutput") != NULL ){ 00311 //////pc.printf("\n\r looking for data \n\r"); 00312 char* p_webdata = strstr(webdata, "groundPidMinOutput"); 00313 p_webdata = p_webdata + strlen("groundPidMinOutput") + 1; 00314 int i = 0; 00315 while(*p_webdata != '&'){ 00316 ////pc.printf("%c", *p_webdata); 00317 _str[i] = *p_webdata; 00318 p_webdata += 1; 00319 i++; 00320 } 00321 ////pc.printf("\n\r"); 00322 _str[i] = '\0'; 00323 //////pc.printf("groundPidMinOutput: %s", _str); 00324 _groundPidMinOutput = atof(_str); 00325 //////pc.printf("\n\r end of looking for data \n\r"); 00326 } 00327 //pid max output 00328 if (strstr(webdata, "groundPidMaxOutput") != NULL ){ 00329 //////pc.printf("\n\r looking for data \n\r"); 00330 char* p_webdata = strstr(webdata, "groundPidMaxOutput"); 00331 p_webdata = p_webdata + strlen("groundPidMaxOutput") + 1; 00332 int i = 0; 00333 while(*p_webdata != '&'){ 00334 ////pc.printf("%c", *p_webdata); 00335 _str[i] = *p_webdata; 00336 p_webdata += 1; 00337 i++; 00338 } 00339 //////pc.printf("\n\r"); 00340 _str[i] = '\0'; 00341 //////pc.printf("groundPidMaxOutput: %s", _str); 00342 _groundPidMaxOutput = atof(_str); 00343 //////pc.printf("\n\r end of looking for data \n\r"); 00344 } 00345 //bias 00346 if (strstr(webdata, "bias") != NULL ){ 00347 //////pc.printf("\n\r looking for data \n\r"); 00348 char* p_webdata = strstr(webdata, "bias"); 00349 p_webdata = p_webdata + strlen("bias") + 1; 00350 int i = 0; 00351 while(*p_webdata != '&'){ 00352 ////pc.printf("%c", *p_webdata); 00353 _str[i] = *p_webdata; 00354 p_webdata += 1; 00355 i++; 00356 } 00357 //////pc.printf("\n\r"); 00358 _str[i] = '\0'; 00359 //////pc.printf("bias: %s", _str); 00360 _bias = atof(_str); 00361 //////pc.printf("\n\r end of looking for data \n\r"); 00362 } 00363 // PID values 00364 if (strstr(webdata, "proportional") != NULL ){ 00365 //////pc.printf("\n\r looking for data \n\r"); 00366 char* p_webdata = strstr(webdata, "proportional"); 00367 p_webdata = p_webdata + strlen("proportional") + 1; 00368 int i = 0; 00369 while(*p_webdata != '&'){ 00370 ////pc.printf("%c", *p_webdata); 00371 _str[i] = *p_webdata; 00372 p_webdata += 1; 00373 i++; 00374 } 00375 //////pc.printf("\n\r"); 00376 _str[i] = '\0'; 00377 //////pc.printf("proportional: %s", _str); 00378 _newP = atof(_str); 00379 //////pc.printf("\n\r end of looking for data \n\r"); 00380 } 00381 if (strstr(webdata, "integral") != NULL){ 00382 //////pc.printf("\n\r looking for data \n\r"); 00383 char* p_webdata = strstr(webdata, "integral"); 00384 p_webdata = p_webdata + strlen("integral") + 1; 00385 int i = 0; 00386 while(*p_webdata != '&'){ 00387 ////pc.printf("%c", *p_webdata); 00388 _str[i] = *p_webdata; 00389 p_webdata += 1; 00390 i++; 00391 } 00392 //////pc.printf("\n\r"); 00393 _str[i] = '\0'; 00394 //////pc.printf("integral: %s", _str); 00395 _newI = atof(_str); 00396 //////pc.printf("\n\r end of looking for data \n\r"); 00397 00398 } 00399 if (strstr(webdata, "derivative") != NULL){ 00400 //////pc.printf("\n\r looking for data \n\r"); 00401 char* p_webdata = strstr(webdata, "derivative"); 00402 p_webdata = p_webdata + strlen("derivative") + 1; 00403 int i = 0; 00404 while(*p_webdata != '&'){ 00405 ////pc.printf("%c", *p_webdata); 00406 _str[i] = *p_webdata; 00407 p_webdata += 1; 00408 i++; 00409 } 00410 //////pc.printf("\n\r"); 00411 _str[i] = '\0'; 00412 //////pc.printf("derivative: %s", _str); 00413 _newD = atof(_str); 00414 //////pc.printf("\n\r end of looking for data \n\r"); 00415 } 00416 if (strstr(webdata, "groundSetPoint") != NULL){ 00417 //////pc.printf("\n\r looking for data \n\r"); 00418 char* p_webdata = strstr(webdata, "groundSetPoint"); 00419 p_webdata = p_webdata + strlen("groundSetPoint") + 1; 00420 int i = 0; 00421 while(*p_webdata != '&'){ 00422 ////pc.printf("%c", *p_webdata); 00423 _str[i] = *p_webdata; 00424 p_webdata += 1; 00425 i++; 00426 } 00427 //////pc.printf("\n\r"); 00428 _str[i] = '\0'; 00429 //////pc.printf("groundSetPoint: %s", _str); 00430 _newGroundSetPoint = atof(_str); 00431 //////pc.printf("\n\r end of looking for data \n\r"); 00432 } 00433 //Go ahead 00434 if (strstr(webdata, "goAhead=1") != NULL ){ 00435 pc.printf("go agead ON \n\r"); 00436 _goAhead = true; 00437 } 00438 if (strstr(webdata, "goAhead=0") != NULL ){ 00439 pc.printf("go agead OFF \n\r"); 00440 _goAhead = false; 00441 } 00442 pc.printf("test \n\r"); 00443 sprintf(channel, "%d",linkID); 00444 if (strstr(webdata, "GET") != NULL) { 00445 pc.printf("GET \n\r"); 00446 servreq=1; 00447 } 00448 if (strstr(webdata, "POST") != NULL) { 00449 pc.printf("POST \n\r"); 00450 servreq=1; 00451 } 00452 webcounter++; 00453 sprintf(webcount, "%d",webcounter); 00454 }else{ 00455 pc.printf("no + \n\r"); 00456 memset(webbuff, '\0', sizeof(webbuff)); 00457 esp.attach(&callback); 00458 weberror=1; 00459 } 00460 00461 } 00462 00463 void startserver(){ 00464 //pc.printf("++++++++++ Resetting ESP ++++++++++\r\n"); 00465 strcpy(cmdbuff,"AT+RST\r\n"); 00466 timeout=8000; 00467 getcount=1000; 00468 SendCMD(); 00469 getreply(); 00470 //pc.printf(replybuff); 00471 //pc.printf("%d",count); 00472 if (strstr(replybuff, "OK") != NULL) { 00473 //////pc.printf("\n++++++++++ Starting Server ++++++++++\r\n"); 00474 strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); // set multiple connections. 00475 timeout=500; 00476 getcount=20; 00477 SendCMD(); 00478 getreply(); 00479 //pc.printf(replybuff); 00480 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port); 00481 timeout=500; 00482 getcount=20; 00483 SendCMD(); 00484 getreply(); 00485 //pc.printf(replybuff); 00486 wait(1); 00487 sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout); 00488 timeout=500; 00489 getcount=50; 00490 SendCMD(); 00491 getreply(); 00492 //pc.printf(replybuff); 00493 wait(5); 00494 //pc.printf("\n Getting Server IP \r\n"); 00495 strcpy(cmdbuff, "AT+CIFSR\r\n"); 00496 timeout=2500; 00497 getcount=200; 00498 while(weberror==0) { 00499 SendCMD(); 00500 getreply(); 00501 if (strstr(replybuff, "0.0.0.0") == NULL) { 00502 weberror=1; // wait for valid IP 00503 } 00504 } 00505 //pc.printf("\n Enter WEB address (IP) found below in your browser \r\n\n"); 00506 //pc.printf("\n The MAC address is also shown below,if it is needed \r\n\n"); 00507 replybuff[strlen(replybuff)-1] = '\0'; 00508 //char* IP = replybuff + 5; 00509 sprintf(webdata,"%s", replybuff); 00510 //SAVE IP ADDRESS 00511 get_ip_address(webdata); 00512 //pc.printf("ip_address: %s\n\r",ip_address); 00513 //pc.printf(webdata); 00514 //pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n"); 00515 esp.attach(&callback); 00516 } else { 00517 //pc.printf("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n"); 00518 while(1) {} 00519 } 00520 t2.reset(); 00521 t2.start(); 00522 } 00523 00524 void SendCMD() 00525 { 00526 esp.printf("%s", cmdbuff); 00527 } 00528 00529 void getreply() 00530 { 00531 memset(replybuff, '\0', sizeof(replybuff)); 00532 t1.reset(); 00533 t1.start(); 00534 replycount=0; 00535 while(t1.read_ms()< timeout && replycount < getcount) { 00536 if(esp.readable()) { 00537 replybuff[replycount] = esp.getc(); 00538 replycount++; 00539 } 00540 } 00541 t1.stop(); 00542 } 00543 00544 void get_ip_address(char web_data[1024]){ 00545 char pre_text[] = "+CIFSR:STAIP,\""; 00546 char* p_text = strstr(webdata, pre_text); 00547 p_text = p_text+strlen(pre_text); 00548 char* p_text2 = strstr(p_text, "\""); 00549 for(int i = 0;;i++){ 00550 if(*(p_text+i) == *p_text2){ 00551 ip_address[i] = '\0'; 00552 break; 00553 } 00554 ip_address[i] = *(p_text+i); 00555 00556 } 00557 }
Generated on Sun Jul 17 2022 22:20:41 by 1.7.2