add max nrfreceivedelay to display

hoverbrettctrl/src/main.cpp

@@ -63,7 +63,13 @@ bool button_down_click=false;
 #define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
 
 
+#define SPEED_COEFFICIENT_NRF   1  // higher value == stronger
+#define STEER_COEFFICIENT_NRF   0.5  // higher value == stronger
 
+#define SPEED_COEFFICIENT_GT   1  // higher value == stronger
+#define STEER_COEFFICIENT_GT   0.5  // higher value == stronger
+
+unsigned long watchdogtimer=0; //set to current millis everytime new good control input is calculated
 
 long last_adcupdated=0;
 #define ADC_UPDATEPERIOD 10 //in ms
@@ -73,6 +79,9 @@ long last_controlupdate = 0;
 float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD
 float filter_nrf_set_steer=0.06;
 
+float filter_stop_set_speed=0.015; //safety stop
+float filter_stop_set_steer=0.06; //safety stop
+
 #define GT_LENGTH_MIN 200 //minimum length for stuff to start happen
 
 #define GT_LENGTH_1_OFFSET -22.5
@@ -157,6 +166,7 @@ nrfdata lastnrfdata;
 long last_nrfreceive = 0; //last time values were received and checksum ok
 unsigned long nrf_delay = 0;
 unsigned long last_nrfreceive_delay=0;
+unsigned long max_last_nrfreceive_delay=0; //maximum value (used for display)
 #define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm
 
 boolean radiosendOk=false;
@@ -172,7 +182,6 @@ long last_send = 0;
 
 int16_t set_speed = 0;
 int16_t set_steer = 0;
-uint8_t out_checksum = 0; //0= disable motors, 255=reserved,  1<=checksum<255
 #define NRFDATA_CENTER 127
 
 //boolean armed = false;
@@ -433,11 +442,13 @@ void loop() {
     if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
       lastpacketOK = true;
       last_nrfreceive_delay=loopmillis-last_nrfreceive; //for display purpose
+      max_last_nrfreceive_delay=max(last_nrfreceive_delay,max_last_nrfreceive_delay);
       last_nrfreceive = loopmillis;
 
+      watchdogtimer=loopmillis; //reset watchdog
 
       //parse commands
-      motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
+      motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 . Used for safety-off when remote released
     }
   }
 
@@ -466,8 +477,7 @@ void loop() {
         set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
 
         //calculate speed l and r from speed and steer
-        #define SPEED_COEFFICIENT_NRF   1  // higher value == stronger
+        
-        #define STEER_COEFFICIENT_NRF   0.5  // higher value == stronger
         int16_t _out_speedl,_out_speedr;
         _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF +  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
         _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF -  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
@@ -493,6 +503,7 @@ void loop() {
   }else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
     //Gametrak Control Code
     motorenabled=true;
+    watchdogtimer=loopmillis; //reset watchdog
     if (gt_length<=GT_LENGTH_MIN){ //let go
       //Serial.println("gametrak released");
       controlmode=MODE_DISARMED;
@@ -536,41 +547,52 @@ void loop() {
     }
 
     //calculate speed l and r from speed and steer
-    #define SPEED_COEFFICIENT_GT   1  // higher value == stronger
+    if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
-    #define STEER_COEFFICIENT_GT   0.5  // higher value == stronger
+      last_controlupdate = loopmillis;
       int16_t _out_speedl,_out_speedr;
       _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT +  set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier;
       _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT -  set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier;
       esc.setSpeed(_out_speedl,_out_speedr);
     }
+  }
 
 
   if (error > 0) { //disarm if error occured
     controlmode = MODE_DISARMED; //force disarmed
   }
 
+  
   if (controlmode == MODE_DISARMED){ //all disarmed
-    esc.setSpeed(0,0);
+    #define WATCHDOG_TIMEOUT 2000
-    set_speed=0; //reset filter
+    if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) {
+      set_speed=0;
       set_steer=0;
     }
+    if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
+      last_controlupdate = loopmillis;
+      set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
+      set_steer = set_steer*(1.0-filter_stop_set_steer);
+      int16_t _out_speedl,_out_speedr;
+      _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF +  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
+      _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF -  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
+      esc.setSpeed(_out_speedl,_out_speedr);
+    }
+  }
 
 
   
   if (esc.sendPending(loopmillis)) {
     
-    //calculate checksum
+    if (!motorenabled) {//motors disabled. Used for safety off when remote released
-    out_checksum = ((uint8_t) ((uint8_t)esc.getCmdL()) * ((uint8_t)esc.getCmdR())); //simple checksum
+      if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
-    if (out_checksum == 0 || out_checksum == 255) {
+        last_controlupdate = loopmillis;
-      out_checksum = 1;  //cannot be 0 or 255 (special purpose)
+        set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
+        set_steer = set_steer*(1.0-filter_stop_set_steer);
+        int16_t _out_speedl,_out_speedr;
+        _out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF +  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
+        _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF -  set_steer * STEER_COEFFICIENT_NRF, -1500, 1500);
+        esc.setSpeed(_out_speedl,_out_speedr);
       }
-  
-    if (!motorenabled) { //disable motors?
-      out_checksum = 0; //checksum=0 disables motors
-    }
-  
-    if (!motorenabled) {//motors disabled
-      esc.setSpeed(0,0);
     }
     last_send = loopmillis;
   
@@ -650,6 +672,7 @@ void updateInputs(unsigned long loopmillis) {
       }
       if (button_down_click) {
         displaymode=DISPLAY_STATS3;
+        max_last_nrfreceive_delay=0; //reset when switching to this display
       }
       break;
     case DISPLAY_MENU:
@@ -856,7 +879,7 @@ void display_show_stats3() {
   display.print(F("looptime=")); display.println(loopmillis-last_loopmillis);
   display.print(F("feedback=")); display.println(esc.getFeedbackInterval());
   
-  display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
+  display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay); display.print(F(" max=")); display.println(max_last_nrfreceive_delay);
   display.print(F("maxdiff=")); display.println(raw_length_maxdiff);