mag auto align if idle

controller.ino

@@ -8,13 +8,20 @@
 //set boot0 back to 0 to run program on powerup
 
 //#define DEBUG
+uint8_t error=0;
+#define IMU_NO_CHANGE 2 //IMU values did not change for too long
+uint8_t imu_no_change_counter=0;
 
 #define PIN_LED PC13
 
 #define SENDPERIOD 20 //ms
 
+#define CONTROLUPDATEPERIOD 10
+long last_controlupdate=0;
+
 #define IMUUPDATEPERIOD 10 //ms
 long last_imuupdated=0;
+#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
 
 
 #include <IMUGY85.h>
@@ -24,8 +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 setYaw=0;
+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
@@ -51,6 +61,7 @@ RF24 radio(PB0,PB1); //ce, cs
 
 // Radio pipe addresses for the 2 nodes to communicate.
 const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
+#define NRF24CHANNEL 75
 
 struct nrfdata {
   uint8_t steer;
@@ -98,6 +109,8 @@ void setup() {
 
   radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
 
+  radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
+  
   radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
   radio.setPayloadSize(8); // optionally, reduce the payload size.  seems to improve reliability
   
@@ -132,15 +145,17 @@ void loop() {
       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        
+        armed=true; //arm at first received packet
+      }
+
       //parse commands
       motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
       if (!motorenabled){ //disable motors?
         armed=false;
       }
 
-      if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==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 not ok?
@@ -185,35 +200,54 @@ void loop() {
     }
   }
 
+  if (error>0){ //disarm if error occured
+    armed=false;
+  }
   if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
     armed=false;
   }
   if (armed){
-    //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
+    if (millis()-last_controlupdate>CONTROLUPDATEPERIOD){
-    //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
+      last_controlupdate=millis();
-    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 );
-    //Test Mag    
-    /*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/2,0,800);
-    yawdiff*=yawdiffsign; //redo sign
       
-    out_steer=(int16_t)( yawdiff );
+      //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
-    Serial.print("Out steer=");
+      out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
-    Serial.println(out_steer);*/
+      
+      //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;
   }
   
 
@@ -251,6 +285,27 @@ void loop() {
 
 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){
+    imu_no_change_counter++;
+    if (imu_no_change_counter>10){
+      error=IMU_NO_CHANGE; 
+      Serial.println("Error: IMU_NO_CHANGE");
+    }
+  }else{
+    imu_no_change_counter=0;
+  }
+  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;
+  old_roll=roll;
+  old_pitch=pitch;
+  old_yaw=yaw;
   //Update Imu and write to variables
   imu.update();
   imu.getAcceleration(&ax, &ay, &az);