hoverbrettctrl/src/main.cpp

@@ -63,13 +63,7 @@ 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
@@ -79,9 +73,6 @@ 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
@@ -166,7 +157,6 @@ 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;
@@ -182,6 +172,7 @@ 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;
@@ -442,13 +433,11 @@ 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 . Used for safety-off when remote released
+      motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0
     }
   }
 
@@ -477,7 +466,8 @@ 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);
@@ -503,7 +493,6 @@ 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;
@@ -547,52 +536,41 @@ void loop() {
     }
 
     //calculate speed l and r from speed and steer
-    if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
+    #define SPEED_COEFFICIENT_GT   1  // higher value == stronger
-      last_controlupdate = loopmillis;
+    #define STEER_COEFFICIENT_GT   0.5  // higher value == stronger
     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
-    #define WATCHDOG_TIMEOUT 2000
+    esc.setSpeed(0,0);
-    if (millis()-watchdogtimer>WATCHDOG_TIMEOUT) {
+    set_speed=0; //reset filter
-      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)) {
     
-    if (!motorenabled) {//motors disabled. Used for safety off when remote released
+    //calculate checksum
-      if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
+    out_checksum = ((uint8_t) ((uint8_t)esc.getCmdL()) * ((uint8_t)esc.getCmdR())); //simple checksum
-        last_controlupdate = loopmillis;
+    if (out_checksum == 0 || out_checksum == 255) {
-        set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
+      out_checksum = 1;  //cannot be 0 or 255 (special purpose)
-        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;
   
@@ -672,7 +650,6 @@ 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:
@@ -879,7 +856,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(" max=")); display.println(max_last_nrfreceive_delay);
+  display.print(F("nrf_delay=")); display.println(last_nrfreceive_delay);
   display.print(F("maxdiff=")); display.println(raw_length_maxdiff);