disarm only at timeout

controller.ino

@@ -73,7 +73,7 @@ struct nrfdata {
 nrfdata lastnrfdata;
 long last_nrfreceive = 0; //last time values were received and checksum ok
 long nrf_delay = 0;
-#define MAX_NRFDELAY 50
+#define MAX_NRFDELAY 100 //ms. maximum time delay at which vehicle will disarm
 
 //command variables
 boolean motorenabled = false; //set by nrfdata.commands
@@ -89,6 +89,7 @@ uint8_t out_checksum=0; //0= disable motors, 255=reserved,  1<=checksum<255
 #define NRFDATA_CENTER 127
 
 boolean armed = false;
+boolean lastpacketOK = false;
 
 
 
@@ -142,26 +143,28 @@ void loop() {
     bool done = false;
     while (!done)
     {
+      lastpacketOK = false; //initialize with false, if checksum ok gets set to true
       digitalWrite(PIN_LED, !digitalRead(PIN_LED));
       done = radio.read( &lastnrfdata, sizeof(nrfdata) );
 
-      if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered        
+      if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered
         armed = true; //arm at first received packet
       }
 
+
+
+
+
+      uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
+      if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
+        lastpacketOK = true;
+        last_nrfreceive = millis();
+
         //parse commands
         motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
         if (!motorenabled) { //disable motors?
           armed = false;
         }
-
-         
-
-      uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
-      if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
-        armed=false; 
-      }else{ //checksum ok
-        last_nrfreceive=millis();
       }
 
 #ifdef DEBUG
@@ -177,6 +180,7 @@ void loop() {
 
       Serial.print("nrfdelay=");
       Serial.print(nrf_delay);
+      Serial.println();
 #endif
 
       //y positive = forward
@@ -205,8 +209,12 @@ void loop() {
   }
   if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
     armed = false;
+    #ifdef DEBUG
+    Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
+    #endif
   }
-  if (armed){
+  if (armed) { //is armed
+    if (lastpacketOK) { //if lastnrfdata is valid
       if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
         last_controlupdate = millis();
 
@@ -229,6 +237,7 @@ void loop() {
         int16_t out_steer_mag = (int16_t)( yawdiff );
 
         float new_magalign_multiplier = map( abs((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering
+        new_magalign_multiplier = 0; //Force mag off
         new_magalign_multiplier = constrain(new_magalign_multiplier, 0.0, 1.0);
 
         magalign_multiplier = min(new_magalign_multiplier, min(1.0, magalign_multiplier + 0.01)); //go down fast, slowly increase
@@ -242,8 +251,15 @@ void loop() {
           Serial.print("Out steer=");
           Serial.println(out_steer);*/
       }
+    }//if pastpacket not ok, keep last out_steer and speed values until disarmed
 
-  }else{ //took too long since last nrf data
+#ifdef DEBUG
+    if (!lastpacketOK)
+      Serial.println("Armed but packet not ok");
+  }
+#endif
+
+} else { //disarmed
   out_steer = 0;
   out_speed = 0;
   setYaw = yaw;
@@ -256,7 +272,9 @@ void loop() {
 if (millis() - last_send > SENDPERIOD) {
   //calculate checksum
   out_checksum = ((uint8_t) ((uint8_t)out_steer) * ((uint8_t)out_speed)); //simple checksum
-    if (out_checksum==0 || out_checksum==255){ out_checksum=1; } //cannot be 0 or 255 (special purpose)
+  if (out_checksum == 0 || out_checksum == 255) {
+    out_checksum = 1;  //cannot be 0 or 255 (special purpose)
+  }
 
   if (!motorenabled) { //disable motors?
     out_checksum = 0; //checksum=0 disables motors
@@ -315,9 +333,9 @@ void updateIMU()
   pitch = imu.getPitch();
   yaw = imu.getYaw();
   /*Directions:
-   * Components on top.
-   * Roll: around Y axis (pointing to the right), left negative
-   * Pitch: around X axis (pointing forward), up positive
-   * Yaw: around Z axis, CCW positive,  0 to 360
+     Components on top.
+     Roll: around Y axis (pointing to the right), left negative
+     Pitch: around X axis (pointing forward), up positive
+     Yaw: around Z axis, CCW positive,  0 to 360
   */
 }