disarm only at timeout

controller.ino

@@ -8,19 +8,19 @@
 //set boot0 back to 0 to run program on powerup
 
 //#define DEBUG
-uint8_t error=0;
+uint8_t error = 0;
 #define IMU_NO_CHANGE 2 //IMU values did not change for too long
-uint8_t imu_no_change_counter=0;
+uint8_t imu_no_change_counter = 0;
 
 #define PIN_LED PC13
 
 #define SENDPERIOD 20 //ms
 
 #define CONTROLUPDATEPERIOD 10
-long last_controlupdate=0;
+long last_controlupdate = 0;
 
 #define IMUUPDATEPERIOD 10 //ms
-long last_imuupdated=0;
+long last_imuupdated = 0;
 #define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
 
 
@@ -31,11 +31,11 @@ long last_imuupdated=0;
 //in qmc5883L library read values changed from uint16_t to int16_t
 
 IMUGY85 imu;
-double ax, ay, az, gx, gy, gz, roll, pitch, yaw,mx,my,mz,ma;
+double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx, my, mz, ma;
-double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw,old_mx,old_my,old_mz,old_ma;
+double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw, old_mx, old_my, old_mz, old_ma;
 
-double setYaw=0;
+double setYaw = 0;
-float magalign_multiplier=0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned
+float magalign_multiplier = 0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned
 
 
 //from left to right. pins at bottom. chips on top
@@ -54,7 +54,7 @@ float magalign_multiplier=0; //how much the magnetometer should influence steeri
 #include "nRF24L01.h"
 #include "RF24.h"
 
-RF24 radio(PB0,PB1); //ce, cs
+RF24 radio(PB0, PB1); //ce, cs
 //SCK D13 (Pro mini), A5 (bluepill)
 //Miso D12 (Pro mini), A6 (bluepill)
 //Mosi D11 (Pro mini), A7 (bluepill)
@@ -71,24 +71,25 @@ struct nrfdata {
 };
 
 nrfdata lastnrfdata;
-long last_nrfreceive=0; //last time values were received and checksum ok
+long last_nrfreceive = 0; //last time values were received and checksum ok
-long nrf_delay=0;
+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
+boolean motorenabled = false; //set by nrfdata.commands
 
 
 
 
-long last_send=0;
+long last_send = 0;
 
-int16_t out_steer=0; //between -1000 and 1000
+int16_t out_steer = 0; //between -1000 and 1000
-int16_t out_speed=0;
+int16_t out_speed = 0;
-uint8_t out_checksum=0; //0= disable motors, 255=reserved,  1<=checksum<255
+uint8_t out_checksum = 0; //0= disable motors, 255=reserved,  1<=checksum<255
 #define NRFDATA_CENTER 127
 
-boolean armed=false;
+boolean armed = false;
+boolean lastpacketOK = false;
 
 
 
@@ -100,7 +101,7 @@ void setup() {
 
 
   pinMode(PIN_LED, OUTPUT);
-  digitalWrite(PIN_LED,HIGH);
+  digitalWrite(PIN_LED, HIGH);
 
 
   Serial.println("Initializing nrf24");
@@ -111,11 +112,11 @@ void setup() {
 
   radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
 
-  radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
+  radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries
   radio.setPayloadSize(8); // optionally, reduce the payload size.  seems to improve reliability
 
   radio.openWritingPipe(pipes[0]); //write on pipe 0
-  radio.openReadingPipe(1,pipes[1]); //read on pipe 1
+  radio.openReadingPipe(1, pipes[1]); //read on pipe 1
 
   radio.startListening();
 
@@ -129,42 +130,44 @@ void setup() {
 
 void loop() {
 
-  if (millis()-last_imuupdated>IMUUPDATEPERIOD){
+  if (millis() - last_imuupdated > IMUUPDATEPERIOD) {
     updateIMU();
-    last_imuupdated=millis();
+    last_imuupdated = millis();
   }
 
   //NRF24
-  nrf_delay=millis()-last_nrfreceive; //update nrf delay
+  nrf_delay = millis() - last_nrfreceive; //update nrf delay
   if ( radio.available() )
   {
     //Serial.println("radio available ...");
     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
+        armed = true; //arm at first received packet
-      }
-
-      //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; 
+      uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
-      }else{ //checksum ok
+      if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
-        last_nrfreceive=millis();
+        lastpacketOK = true;
+        last_nrfreceive = millis();
+
+        //parse commands
+        motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
+        if (!motorenabled) { //disable motors?
+          armed = false;
+        }
       }
 
-      #ifdef DEBUG
+#ifdef DEBUG
       Serial.print("Received:");
       Serial.print(" st=");
       Serial.print(lastnrfdata.steer);
@@ -177,147 +180,162 @@ void loop() {
 
       Serial.print("nrfdelay=");
       Serial.print(nrf_delay);
-      #endif
+      Serial.println();
+#endif
 
       //y positive = forward
       //x positive = right
 
       /*
-      setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
+        setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
-      while (setYaw<0){
+        while (setYaw<0){
         setYaw+=360;
-      }
+        }
-      while (setYaw>=360){
+        while (setYaw>=360){
         setYaw-=360;
-      }*/
+        }*/
 
       /*
-      Serial.print("setYaw=");
+        Serial.print("setYaw=");
-      Serial.print(setYaw);
+        Serial.print(setYaw);
-      Serial.print(" Yaw=");
+        Serial.print(" Yaw=");
-      Serial.println(yaw);*/
+        Serial.println(yaw);*/
 
     }
   }
 
-  if (error>0){ //disarm if error occured
+  if (error > 0) { //disarm if error occured
-    armed=false;
+    armed = false;
   }
-  if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
+  if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
-    armed=false;
+    armed = false;
-  }
-  if (armed){
-    if (millis()-last_controlupdate>CONTROLUPDATEPERIOD){
-      last_controlupdate=millis();
-      
-      //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
-      //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
-      out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000
-      out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
-      
-      //align to compass   
-      double yawdiff=(setYaw-180)-(yaw-180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
-      yawdiff += (yawdiff>180) ? -360 : (yawdiff<-180) ? 360 : 0;
-      //yawdiff/=2;
-      int yawdiffsign=1;
-      if (yawdiff<0){
-        yawdiffsign=-1;
-      }
-      yawdiff=yawdiff*yawdiff; //square
-      yawdiff=constrain(yawdiff*1 ,0,800);
-      yawdiff*=yawdiffsign; //redo sign
-      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=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
-      magalign_multiplier=constrain(magalign_multiplier, 0.0,1.0); //safety constrain again
-  
-      out_steer = out_steer*(1-magalign_multiplier) + out_steer_mag*magalign_multiplier;
-  
-      setYaw=setYaw*magalign_multiplier + yaw*(1-magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
-      
-      /*
-      Serial.print("Out steer=");
-      Serial.println(out_steer);*/
-    }
-    
-  }else{ //took too long since last nrf data
-    out_steer=0;
-    out_speed=0;
-    setYaw=yaw;
-    magalign_multiplier=0;
-  }
-  
-
-  
-
-  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 (!motorenabled){ //disable motors?
-      out_checksum=0; //checksum=0 disables motors
-    }
-    
-    Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
-    Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
-    Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
-    last_send=millis();
-
-
     #ifdef DEBUG
-    Serial.print(" steer=");
+    Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
-    Serial.print(out_steer);
-    Serial.print(" speed=");
-    Serial.print(out_speed);
-    Serial.print(" checksum=");
-    Serial.print(out_checksum);
-    
-    Serial.println();
     #endif
   }
+  if (armed) { //is armed
+    if (lastpacketOK) { //if lastnrfdata is valid
+      if (millis() - last_controlupdate > CONTROLUPDATEPERIOD) {
+        last_controlupdate = millis();
+
+        //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
+        //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
+        out_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
+        out_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );
+
+        //align to compass
+        double yawdiff = (setYaw - 180) - (yaw - 180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
+        yawdiff += (yawdiff > 180) ? -360 : (yawdiff < -180) ? 360 : 0;
+        //yawdiff/=2;
+        int yawdiffsign = 1;
+        if (yawdiff < 0) {
+          yawdiffsign = -1;
+        }
+        yawdiff = yawdiff * yawdiff; //square
+        yawdiff = constrain(yawdiff * 1 , 0, 800);
+        yawdiff *= yawdiffsign; //redo sign
+        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
+        magalign_multiplier = constrain(magalign_multiplier, 0.0, 1.0); //safety constrain again
+
+        out_steer = out_steer * (1 - magalign_multiplier) + out_steer_mag * magalign_multiplier;
+
+        setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
+
+        /*
+          Serial.print("Out steer=");
+          Serial.println(out_steer);*/
+      }
+    }//if pastpacket not ok, keep last out_steer and speed values until disarmed
+
+#ifdef DEBUG
+    if (!lastpacketOK)
+      Serial.println("Armed but packet not ok");
+  }
+#endif
+
+} else { //disarmed
+  out_steer = 0;
+  out_speed = 0;
+  setYaw = yaw;
+  magalign_multiplier = 0;
+}
+
+
+
+
+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 (!motorenabled) { //disable motors?
+    out_checksum = 0; //checksum=0 disables motors
+  }
+
+  Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
+  Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
+  Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
+  last_send = millis();
+
+
+#ifdef DEBUG
+  Serial.print(" steer=");
+  Serial.print(out_steer);
+  Serial.print(" speed=");
+  Serial.print(out_speed);
+  Serial.print(" checksum=");
+  Serial.print(out_checksum);
+
+  Serial.println();
+#endif
+}
 
 }
 
 
 void updateIMU()
 {
-  if (old_ax==ax && old_ay==ay && old_az==az && old_gx==gx && old_gy==gy && old_gz==gz && old_mx==mx && old_my==my && old_mz==mz){
+  if (old_ax == ax && old_ay == ay && old_az == az && old_gx == gx && old_gy == gy && old_gz == gz && old_mx == mx && old_my == my && old_mz == mz) {
     imu_no_change_counter++;
-    if (imu_no_change_counter>10){
+    if (imu_no_change_counter > 10) {
-      error=IMU_NO_CHANGE; 
+      error = IMU_NO_CHANGE;
       Serial.println("Error: IMU_NO_CHANGE");
     }
-  }else{
+  } else {
-    imu_no_change_counter=0;
+    imu_no_change_counter = 0;
   }
-  old_ax=ax;
+  old_ax = ax;
-  old_ay=ay;
+  old_ay = ay;
-  old_az=az;
+  old_az = az;
-  old_gx=gx;
+  old_gx = gx;
-  old_gy=gy;
+  old_gy = gy;
-  old_gz=gz;
+  old_gz = gz;
-  old_mx=mx;
+  old_mx = mx;
-  old_my=my;
+  old_my = my;
-  old_mz=mz;
+  old_mz = mz;
-  old_roll=roll;
+  old_roll = roll;
-  old_pitch=pitch;
+  old_pitch = pitch;
-  old_yaw=yaw;
+  old_yaw = yaw;
   //Update Imu and write to variables
   imu.update();
   imu.getAcceleration(&ax, &ay, &az);
   imu.getGyro(&gx, &gy, &gz);
-  imu.getMag(&mx, &my, &mz,&ma); //calibration data such as bias is set in IMUGY85.h
+  imu.getMag(&mx, &my, &mz, &ma); //calibration data such as bias is set in IMUGY85.h
   roll = imu.getRoll();
   pitch = imu.getPitch();
   yaw = imu.getYaw();
   /*Directions:
-   * Components on top.
+     Components on top.
-   * Roll: around Y axis (pointing to the right), left negative
+     Roll: around Y axis (pointing to the right), left negative
-   * Pitch: around X axis (pointing forward), up positive
+     Pitch: around X axis (pointing forward), up positive
-   * Yaw: around Z axis, CCW positive,  0 to 360
+     Yaw: around Z axis, CCW positive,  0 to 360
-   */
+  */
 }