From 26353508aa363ce302370cc06821cba6d48d3668 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 20 Mar 2019 23:29:09 +0100 Subject: [PATCH] mag auto align if idle --- controller.ino | 103 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 79 insertions(+), 24 deletions(-) diff --git a/controller.ino b/controller.ino index 40147bb..1f7aaef 100644 --- a/controller.ino +++ b/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 @@ -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; @@ -97,6 +108,8 @@ void setup() { radio.begin(); 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 ); - //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 ); - //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; + 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);*/ } - yawdiff=yawdiff*yawdiff; //square - yawdiff=constrain(yawdiff/2,0,800); - yawdiff*=yawdiffsign; //redo sign - - out_steer=(int16_t)( yawdiff ); - */ - /* - 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);