YMFC-32-C Drone - Enhanced Telemetry System

1///////////////////////////////////////////////////////////////////////////////////////
2//Terms  of use
3///////////////////////////////////////////////////////////////////////////////////////
4//THE  SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
5//IMPLIED,  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
6//FITNESS FOR  A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
7//AUTHORS OR  COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
8//LIABILITY, WHETHER  IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
9//OUT OF OR IN CONNECTION  WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
10//THE SOFTWARE.
11///////////////////////////////////////////////////////////////////////////////////////
12
13#include  <global.h>
14#include <base64.h>
15#include <WebSocketClient.h>
16#include <WebSocketServer.h>
17#define  _WIFININA_LOGLEVEL_       1
18#include <WiFiNINA_Generic.h>
19#include "network_parameters.h"
20
21//  global var
22char ssid[] = SECRET_SSID;
23char pass[] = SECRET_PASS;
24const  byte IPLastByte  = 99;
25const short webPort     = 80;
26const short socketPort  = 8080;
27
28// WiFi
29WiFiServer      webServer(webPort);
30WiFiServer      socketServer(socketPort);
31WebSocketServer  webSocketServer;
32WiFiClient      socketClient;
33
34// Console Attached
35#ifndef  TerminalAttached
36    // true = terminal attached (send serial messages), false  = no terminal attached no messages 
37    #define TerminalAttached  false
38#endif
39
40//  Buffer Constant Both sides must Match
41#ifndef Receive_Buffer_Size 55
42    //  Receiver Buffer Size 
43    #define Receive_Buffer_Size   55
44#endif
45
46//  Buffer Constant Both sides must Match
47#ifndef Send_Buffer_Size
48    // Send  Buffer Size 
49    #define Send_Buffer_Size   6
50#endif
51
52// telemetry
53uint8_t  telemetry_lost; 
54uint8_t check_byte, temp_byte;
55
56//from YMC-32
57uint8_t  rec_telemetry_last_byte = Receive_Buffer_Size;                                                               //this  is the last byte of the receivers telemetry buffer they have to match the YMC-32's  TX
58uint8_t receive_buffer[Receive_Buffer_Size + 1], receive_buffer_counter, receive_byte_previous,  receive_byte, receive_state;              // rec_telemetry_last_byte + 1
59
60//to  YMC-32                                              
61uint8_t send_telemetry_last_byte  = Send_Buffer_Size;                                                          //this  is the last byte of the transmitters telemetry buffer they have to match the YMC-32's  RX  
62uint8_t telemetry_send_byte, telemetry_loop_counter, ready_to_send;
63uint32_t  telemetry_buffer_byte;
64
65// YMC-32 commands
66uint8_t ymc32_command;
67float  ymc32_fval;
68
69uint8_t error, alarm_sound, flight_mode;
70uint8_t alarm_silence;
71uint8_t  start_byte, flight_timer_start;
72uint8_t hours,minutes,seconds;
73uint8_t heading_lock;
74uint8_t  number_used_sats;
75uint8_t fix_type;
76
77uint8_t debug_byte;
78
79int8_t  page, previous_page;
80
81uint32_t current_receive, current_receive_min, current_receive_max,  last_receive, next_sound, flight_timer, flight_timer_previous, flight_timer_from_start,  flight_time_from_eeprom;
82uint32_t hours_flight_time, minutes_flight_time, seconds_flight_time;
83int32_t  loop_timer, loop_timer_min, loop_timer_max, l_lat_gps, l_lon_gps;
84
85int16_t  temperature,temperature_min,temperature_max, button_push, button_store,roll_angle,roll_angle_min,roll_angle_max,  pitch_angle, pitch_angle_min, pitch_angle_max;
86int16_t altitude_meters, altitude_meters_min,  altitude_meters_max;
87uint16_t key_press_timer;
88int16_t manual_takeoff, takeoff_throttle;
89uint16_t  actual_compass_heading;
90
91float battery_voltage,battery_voltage_min,battery_voltage_max;
92
93float  pid_p_gain_roll;               //Gain setting for the pitch and roll P-controller
94float  pid_i_gain_roll;              //Gain setting for the pitch and roll I-controller
95float  pid_d_gain_roll;              //Gain setting for the pitch and roll D-controller
96float  pid_p_gain_yaw;                //Gain setting for the pitch P-controller (default  = 4.0).
97float pid_i_gain_yaw;               //Gain setting for the pitch I-controller  (default = 0.02).
98float pid_d_gain_yaw;                //Gain setting for the  pitch D-controller (default = 0.0).
99float pid_p_gain_altitude;           //Gain  setting for the altitude P-controller (default = 1.4).
100float pid_i_gain_altitude;           //Gain setting for the altitude I-controller (default = 0.2).
101float  pid_d_gain_altitude;          //Gain setting for the altitude D-controller (default  = 0.75).
102
103byte led;
104
105// Background
106boolean Backgroundinit = true;
107
108//  Return to Home
109uint8_t return_to_home_command;
110
111// YMC32 webpage, gzipped  and base64 encoding to save space
112char webpage_base64[] = "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";
113
114void  printWifiStatus()
115{
116  if(TerminalAttached) {
117    Serial.print("SSID: ");  Serial.println(WiFi.SSID());
118    Serial.print("Signal strength (RSSI): ");  Serial.print(WiFi.RSSI()); Serial.println(" dBm");
119    Serial.print("IP address:  "); Serial.println(WiFi.localIP());
120    Serial.print("Gateway: "); Serial.println(WiFi.gatewayIP());
121    Serial.print("Netmask: "); Serial.println(WiFi.subnetMask());
122    Serial.print("Webpage  is at http://"); Serial.print(WiFi.localIP()); Serial.println("/");
123    Serial.print("Websocket  is at http://"); Serial.print(WiFi.localIP()); Serial.println(":" + (String)socketPort  + "/");
124  }
125}
126
127void WiFiConnect() 
128{
129  while (WiFi.status()  != WL_CONNECTED) 
130  {
131   if(TerminalAttached) {
132       Serial.println("Connecting  to " + (String)ssid + " ...");
133   }
134    WiFi.begin(ssid, pass);
135    delay(5000);
136  }
137  
138  IPAddress IP = WiFi.localIP();
139  IP[3] = IPLastByte;
140  
141  WiFi.config(IP, WiFi.gatewayIP(), WiFi.gatewayIP(), WiFi.subnetMask());
142 if(TerminalAttached)  {
143   Serial.println("Connected to " + (String)ssid);
144 }
145  
146  webServer.begin();
147  socketServer.begin();
148 if(TerminalAttached) {
149   printWifiStatus();
150  }
151  //WiFi.lowPowerMode();
152}
153
154void setup() {
155  pinMode(LED_BUILTIN,  OUTPUT);
156  pinMode(0, OUTPUT);                                   // Green LED
157
158  if(TerminalAttached) {  
159    Serial.begin(57600);
160    delay(5000);
161  }
162  Serial1.begin(9600);  
163  delay(250);  
164
165  led = 0;
166  ready_to_send  = 0;
167  receive_state = 1;
168  check_byte = 0;
169
170  // check and make sure  we are using the lastest Firmware
171  String fv = WiFi.firmwareVersion();
172
173  // Serial port initialization
174  if(TerminalAttached) {
175     Serial.println("\
176YMFC-32  - MRK1010 Receiver/Web Server Started");
177     Serial.println("Version " +  String(WIFININA_GENERIC_VERSION));
178
179     // check and make sure we are using  the lastest Firmware
180     if (fv < WIFI_FIRMWARE_LATEST_VERSION)
181     {
182         Serial.print("Your current firmware NINA FW v");
183         Serial.println(fv);
184         Serial.print("Please upgrade the firmware to NINA FW v");
185         Serial.println(WIFI_FIRMWARE_LATEST_VERSION);
186     }
187 }
188}
189
190void loop() {
191  if(WiFi.status() != WL_CONNECTED)
192  {
193     if(TerminalAttached) {
194         Serial.println("Lost WiFi connection");
195     }
196      WiFi.end();
197      WiFiConnect();
198  }
199
200  WiFiClient webClient  = webServer.available();
201
202  if(webClient.connected())
203  {
204     if(TerminalAttached)  {
205         Serial.print("New client: "); Serial.print(webClient.remoteIP());  Serial.print(":"); Serial.println(webClient.remotePort());
206     }
207      String  header = "";
208
209      while(webClient.available())
210      {
211          char  ch = webClient.read();
212
213          if (ch != '\r') 
214          {
215          header  += ch;
216          }
217      
218          if (header.substring(header.length()  - 2) == "\
219\
220") 
221          {
222         if(TerminalAttached) {
223             Serial.print(header.substring(0,  header.length() - 1));
224         }
225        
226          if (header.indexOf("GET  / HTTP") > -1) 
227          {
228              const int webpage_base64_length  = sizeof(webpage_base64);
229              const int webpage_gz_length = base64_dec_len(webpage_base64,  webpage_base64_length);
230              char webpage_gz[webpage_gz_length];
231              base64_decode(webpage_gz, webpage_base64, webpage_base64_length);
232              int packetsize = 1024;
233              int done = 0;
234              
235              webClient.println("HTTP/1.1 200 OK\
236Content-Type: text/html\
237Content-Encoding:  gzip\
238");
239          
240              while (webpage_gz_length > done) 
241              {
242                  if (webpage_gz_length - done < packetsize)  {
243              
244                  packetsize = webpage_gz_length - done;
245                  }
246            
247                  webClient.write(webpage_gz  + done, packetsize * sizeof(char));
248                  done = done + packetsize;
249              }
250             if(TerminalAttached) {
251                 Serial.println("--Interface  webpage sent");
252             }
253          } 
254          else 
255          {
256              webClient.println("HTTP/1.1 404 Not Found\
257Content-Type: text/plain;  charset=utf-8\
258\
259404 Not Found\
260");
261             if(TerminalAttached) {
262                 Serial.println("--Page not found");
263             }
264          }
265        
266          webClient.stop();
267         if(TerminalAttached) {
268             Serial.println("--Client  disconnected");
269         }
270      }
271    }
272  }
273
274  if(!socketClient.connected())  
275  {
276    socketClient = socketServer.available();
277    
278    if (socketClient.connected()  && webSocketServer.handshake(socketClient)) 
279    {
280       if(TerminalAttached)  {
281           Serial.print("\
282--Websocket connected to: ");
283           Serial.print(socketClient.remoteIP());
284           Serial.print(":");
285           Serial.println(socketClient.remotePort());
286       }
287    } 
288    else 
289    {
290        socketClient.stop();
291        delay(100);
292    }
293  }
294
295  if(socketClient.connected()) 
296  {
297    // Background  Init - setup the background tasks, runs only once
298    if(Backgroundinit == true)  {
299        Backgroundinit = false;
300
301       String data = webSocketServer.getData();
302       if(TerminalAttached) {
303           Serial.println("Websocket Flushed");
304       }
305
306        // flush the serial1 port - Arduion to Arduino comms
307        while(Serial1.available()) {
308            char ch = Serial1.read();
309        }
310       if(TerminalAttached) {
311           Serial.println("Serial  Port 1 Flushed");
312           Serial.println("Background Init Complete");
313       }
314    }
315    
316    // Background Process 1
317    // see if we have  a command/request from the user 
318    String data = webSocketServer.getData();
319    if (data.length() > 0) 
320    {
321      //String cmd = data.substring(0, data.indexOf(":"));
322      //String setting = data.substring(data.indexOf(":") + 1);
323      // get  tools to parse incoming request
324      char buf[data.length()+1];
325      data.toCharArray(buf,  sizeof(buf));
326      char *token;
327      char *pter = buf;
328      byte i =  0;
329      String cmd;
330      String subcommand;
331      String usrVal; 
332      while ((token = strtok_r(pter, ":", &pter)))
333      {
334        switch(i)  {
335          case 0:
336          cmd = token;
337        break;
338          case  1:
339          subcommand = token;
340        break;
341          case 2:
342          usrVal  = token;
343        break;
344        }
345        i++;
346      }
347     if(TerminalAttached)  {
348       Serial.println("CMD: " + String(cmd));
349       Serial.println("Subcommand:  " + String(subcommand));
350       Serial.println("UsrVal: " + String(usrVal));
351     }
352
353      // process command
354      switch (cmd.toInt()) {
355        //do  something (1-9 are the PID buttons - they don't get sent, they are added to 20 and  21)
356        // minus
357        case 20:
358            subcommand = "Minus";
359            break;
360        // plus
361        case 21:
362            subcommand  = "Plus";
363            break;
364        // user command
365        case 30:
366            if(subcommand == "Ledon") {
367                digitalWrite(0, HIGH);
368                webSocketServer.sendData("R:" + cmd + " - " + subcommand);
369            } else if (subcommand == "Ledoff") {
370                digitalWrite(0,  LOW);
371                webSocketServer.sendData("R:" + cmd + " - " + subcommand);
372            }else if (subcommand == "Pidr") {
373                webSocketServer.sendData("R:"  + String(pid_p_gain_roll) + ":" + String(pid_i_gain_roll) + ":" + String(pid_d_gain_roll));
374            }else if (subcommand == "Pidy") {
375                webSocketServer.sendData("R:"  + String(pid_p_gain_yaw) + ":" + String(pid_i_gain_yaw) + ":" + String(pid_d_gain_yaw));
376            } else if (subcommand == "Pida") {
377                webSocketServer.sendData("R:"  + String(pid_p_gain_altitude) + ":" + String(pid_i_gain_altitude) + ":" + String(pid_d_gain_altitude));
378            } else if (subcommand == "Rst") {
379              reset_data();
380              webSocketServer.sendData("R:Reset Sent");
381            } else  {
382                webSocketServer.sendData("E:" + cmd + " - " + subcommand);
383            }
384            break;
385        // user command
386        case 40:
387            if (subcommand == "CRP") {
388              ymc32_command = 1;
389              init_telemetry_data(usrVal);
390              webSocketServer.sendData("P:"  + String(pid_p_gain_roll) + ">>" + String(usrVal));             
391            }  else if (subcommand == "CRI") {
392              ymc32_command = 2;
393              init_telemetry_data(usrVal);
394              webSocketServer.sendData("P:" + String(pid_i_gain_roll) + ">>"  + String(usrVal));
395            } else if (subcommand == "CRD") {
396              ymc32_command  = 3;
397              init_telemetry_data(usrVal);
398              webSocketServer.sendData("P:"  + String(pid_d_gain_roll) + ">>" + String(usrVal));
399            } else if (subcommand  == "CYP") {
400              ymc32_command = 4;
401              init_telemetry_data(usrVal);
402              webSocketServer.sendData("P:" + String(pid_p_gain_yaw) + ">>"  + String(usrVal));
403            } else if (subcommand == "CYI") {
404              ymc32_command  = 5;
405              init_telemetry_data(usrVal);
406               webSocketServer.sendData("P:"  + String(pid_i_gain_yaw) + ">>" + String(usrVal));
407           } else if (subcommand  == "CYD") {
408              ymc32_command = 6;
409              init_telemetry_data(usrVal);
410              webSocketServer.sendData("P:" + String(pid_d_gain_yaw) + ">>"  + String(usrVal));
411            } else if (subcommand == "CAP") {
412              ymc32_command  = 7;
413              init_telemetry_data(usrVal);
414              webSocketServer.sendData("P:"  + String(pid_p_gain_altitude) + ">>" + String(usrVal));
415            } else  if (subcommand == "CAI") {
416              ymc32_command = 8;
417              init_telemetry_data(usrVal);
418              webSocketServer.sendData("P:" + String(pid_i_gain_altitude) + ">>"  + String(usrVal));
419            } else if (subcommand == "CAD") {
420              ymc32_command  = 9;
421              init_telemetry_data(usrVal);
422              webSocketServer.sendData("P:"  + String(pid_d_gain_altitude) + ">>" + String(usrVal));
423            } else  {
424                webSocketServer.sendData("E:" + cmd + " - " + subcommand);
425            }
426            break;
427            case 50:
428                if  (subcommand == "TTC") {
429                  ymc32_command = 10;
430                  init_telemetry_data("0");
431                  webSocketServer.sendData("R:" + cmd + " - MTT Sent");
432                }
433                if (subcommand == "SPD") {
434                  ymc32_command  = 11;
435                  init_telemetry_data("0");
436                  webSocketServer.sendData("R:"  + cmd + " - Save PID Sent");
437                }
438                if (subcommand  == "RTH") {
439                  ymc32_command = 12;
440                  init_telemetry_data("0");
441                  webSocketServer.sendData("R:" + cmd + " - RTH Sent");
442                }
443                if (subcommand == "RDC") {
444                  ymc32_command  = 99;
445                  init_telemetry_data("0");
446                  webSocketServer.sendData("R:"  + cmd + " - Drop Requested");
447                }
448            break;
449        default:
450            break;
451      }
452
453      // process response
454      switch (cmd.toInt())  {
455          // minus response
456          case 20:
457              webSocketServer.sendData("R:"  + cmd + " - " + subcommand);
458              break;
459          // plus response
460          case 21:
461              webSocketServer.sendData("R:" + cmd + " -  " + subcommand);
462              break;
463          case 30:
464              break;
465          case 40:
466              break;
467          case 50:
468              break;
469          default:
470              webSocketServer.sendData("E:" + cmd + " -  " + subcommand);
471              break;
472        }
473    }
474  }
475
476    //  Background Process 2
477    if(Serial1.available()) {                                                //If  there are bytes available.
478      receive_byte = Serial1.read();                                       //Load  them in the received_buffer array.
479        switch (receive_state) {
480            //  waiting for start
481            case 1:
482                if(receive_byte == 'B')  {
483                    if(receive_byte_previous == 'J') {
484                        receive_buffer_counter  = 2;
485                        receive_buffer[0] = 'J';
486                        receive_buffer[1]  = 'B';
487                        receive_state = 2;
488                    }
489                }
490                receive_byte_previous = receive_byte;
491            break;
492            // building buffer
493            case 2:
494                receive_buffer[receive_buffer_counter++]  = receive_byte;
495                // see if we have a valid buffer
496                if(receive_buffer_counter  == rec_telemetry_last_byte) {
497                    check_byte = 0;                                                                         //Reset  the check_byte variable.
498                    for(temp_byte=0;temp_byte < (rec_telemetry_last_byte  - 1); temp_byte ++) {
499                        check_byte ^= receive_buffer[temp_byte];                                            //Calculate the check_byte.
500                    }
501                    // valid buffer
502                    if(check_byte == receive_buffer[(rec_telemetry_last_byte  - 1)]) {
503                      get_data();                                                           //If  there are two start signatures detected there could be a complete data set available.
504                      process_data();
505                     if(TerminalAttached)  {
506                       print_telemetry_data();
507                     }
508                      
509                      // send status up date to webserver
510                      if(socketClient.connected()) {
511                        webpage_data();
512                      }
513      
514                    } else {
515                        receive_byte_previous  = receive_byte;
516                        receive_state = 1;
517                    }
518                } else if (receive_buffer_counter > rec_telemetry_last_byte) {
519                    receive_byte_previous = receive_byte;
520                    receive_state  = 1;
521                }
522            break;
523        }
524    }
525
526  //  Background Process 3
527  // see if we have something to send, if so do it
528  if(ready_to_send  == 1) {
529    send_telemetry_data();
530  }
531  
532  // Background Process 4
533  // background delay and heart beat
534  if(led == 0) {
535    led = 1;
536    digitalWrite(LED_BUILTIN,  HIGH);
537  } else {
538    led = 0;
539    digitalWrite(LED_BUILTIN, LOW);
540  }
541}
542
543//When there are two start signatures received the received data  between them can be tested to se if it is a valid data stream.
544void get_data(void){
545    current_receive = millis() - last_receive;
546    last_receive = millis();                                                              //Remember  when this reception has arrived.
547    //In the following lines the different variables  are restored from the valid data stream.
548    //The name of the variables are  the same as in the YMFC-32 flight controller program.
549    error = receive_buffer[2];
550    flight_mode = receive_buffer[3];
551    debug_byte = receive_buffer[4];
552    temperature = receive_buffer[5] | receive_buffer[6] << 8;
553    roll_angle  = receive_buffer[7] - 100;
554    pitch_angle = receive_buffer[8] - 100;
555    start_byte  = receive_buffer[9];
556    altitude_meters = (receive_buffer[10] | receive_buffer[11]  << 8) - 1000;
557    takeoff_throttle = receive_buffer[12] | receive_buffer[13]  << 8;
558    actual_compass_heading = receive_buffer[14] | receive_buffer[15] <<  8;
559    heading_lock = receive_buffer[16];
560    number_used_sats = receive_buffer[17];
561    fix_type = receive_buffer[18];
562    l_lat_gps = (int32_t)receive_buffer[19]  | (int32_t)receive_buffer[20] << 8 | (int32_t)receive_buffer[21] << 16 | (int32_t)receive_buffer[22]  << 24;
563    l_lon_gps = (int32_t)receive_buffer[23] | (int32_t)receive_buffer[24]  << 8 | (int32_t)receive_buffer[25] << 16 | (int32_t)receive_buffer[26] << 24;
564    loop_timer = (int32_t)receive_buffer[27] | (int32_t)receive_buffer[28] << 8  | (int32_t)receive_buffer[29] << 16 | (int32_t)receive_buffer[30] << 24;
565    pid_p_gain_roll  = (float)(receive_buffer[31] | receive_buffer[32] << 8)/1000;
566    pid_i_gain_roll  = (float)(receive_buffer[33] | receive_buffer[34] << 8)/1000;
567    pid_d_gain_roll  = (float)(receive_buffer[35] | receive_buffer[36] << 8)/1000;
568    pid_p_gain_yaw  = (float)(receive_buffer[37] | receive_buffer[38] << 8)/1000;
569    pid_i_gain_yaw  = (float)(receive_buffer[39] | receive_buffer[40] << 8)/1000;
570    pid_d_gain_yaw  = (float)(receive_buffer[41] | receive_buffer[42] << 8)/1000;
571    pid_p_gain_altitude  = (float)(receive_buffer[43] | receive_buffer[44] << 8)/1000;
572    pid_i_gain_altitude  = (float)(receive_buffer[45] | receive_buffer[46] << 8)/1000;
573    pid_d_gain_altitude  = (float)(receive_buffer[47] | receive_buffer[48] << 8)/1000;
574    battery_voltage  = (float)(receive_buffer[49] | receive_buffer[50] << 8)/100;
575    manual_takeoff  = receive_buffer[51] | receive_buffer[52] << 8;
576    return_to_home_command =  receive_buffer[53];
577}
578
579//Process the data for the Web page
580void process_data(void){
581    //System Status
582    //create battery min and max values
583    if((battery_voltage  <= battery_voltage_min) | (battery_voltage_min == 0)) {
584    battery_voltage_min  = battery_voltage;
585    }
586    if(battery_voltage >= battery_voltage_max) {
587    battery_voltage_max = battery_voltage;
588    }
589
590    //telemetry processing  time
591    if((current_receive <=  current_receive_min) | ( current_receive_min  == 0)) {
592       current_receive_min =  current_receive;
593    }
594    if(current_receive  >=  current_receive_max) {
595       current_receive_max =  current_receive;
596    }
597
598    //loop_timer processing time
599    if((loop_timer <=  loop_timer_min)  | ( loop_timer_min == 0)) {
600       loop_timer_min =  loop_timer;
601    }
602    if(loop_timer >=  loop_timer_max) {
603       loop_timer_max =  loop_timer;
604    }
605
606    // Flight Status
607    //Roll angle
608    if((roll_angle <=  roll_angle_min)  | ( roll_angle_min == 0)) {
609       roll_angle_min =  roll_angle;
610    }
611    if(roll_angle >=  roll_angle_max) {
612       roll_angle_max =  roll_angle;
613    }
614
615    //Pitch angle
616    if((pitch_angle <=  pitch_angle_min) | ( pitch_angle_min  == 0)) {
617       pitch_angle_min =  pitch_angle;
618    }
619    if(pitch_angle  >=  pitch_angle_max) {
620       pitch_angle_max =  pitch_angle;
621    }
622
623    //Altitude Meters
624    if((altitude_meters <=  altitude_meters_min) | ( altitude_meters_min  == 0)) {
625       altitude_meters_min =  altitude_meters;
626    }
627    if(altitude_meters  >=  altitude_meters_max) {
628       altitude_meters_max =  altitude_meters;
629    }
630
631    //Temperature
632    if((temperature <=  temperature_min) | ( temperature_min  == 0)) {
633       temperature_min =  temperature;
634    }
635    if(temperature  >=  temperature_max) {
636       temperature_max =  temperature;
637    }
638}
639
640//Process  the data for the Web page
641void reset_data(void){
642    ymc32_command = 0;
643    ymc32_fval = 0;
644    check_byte = 0;
645    ready_to_send = 1; 
646    battery_voltage_min  = battery_voltage_max = 0;
647    current_receive_min = current_receive_max = 0;
648    loop_timer_min = loop_timer_max = 0;
649    roll_angle_min = roll_angle_max  = 0;
650    pitch_angle_min = pitch_angle_max = 0;
651    altitude_meters_min =  altitude_meters_max = 0;
652    temperature_min = temperature_max = 0;
653}
654