HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

Files at this revision

API Documentation at this revision

Revision 129:a76be8380bb2, committed 2021-11-29

Comitter:
NaotoMorita
Date:
Mon Nov 29 09:45:44 2021 +0000
Parent:
127:d73a6233ee4b
Child:
130:3a0ab2affb0f
Child:
132:896ad37b534b
Commit message:
autopilot mod

Changed in this revision

Autopilot.lib Show annotated file Show diff for this revision Revisions of this file
global.cpp Show annotated file Show diff for this revision Revisions of this file
global.hpp Show annotated file Show diff for this revision Revisions of this file
run.cpp Show annotated file Show diff for this revision Revisions of this file
servo.cpp Show annotated file Show diff for this revision Revisions of this file
setup.cpp Show annotated file Show diff for this revision Revisions of this file
solaESKF.lib Show annotated file Show diff for this revision Revisions of this file
transferData.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/Autopilot.lib	Mon Nov 22 09:51:51 2021 +0000
+++ b/Autopilot.lib	Mon Nov 29 09:45:44 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/Autopilot/#6c3e1d574f86
+https://os.mbed.com/teams/HAPSRG/code/Autopilot/#51a0fedd7745

--- a/global.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/global.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -48,6 +48,7 @@
 Vector3 pi;
 float palt;
 float palt0;
+float dynaccnorm2;
 int itow_status = 0;
 bool gpsUpdateFlag = false;
 bool gpsLlh0Fixed = false;

--- a/global.hpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/global.hpp	Mon Nov 29 09:45:44 2021 +0000
@@ -103,6 +103,7 @@
 extern Vector3 pi;
 extern float palt;
 extern float palt0;
+extern float dynaccnorm2;
 extern int itow_status;
 extern bool gpsUpdateFlag;
 extern bool gpsLlh0Fixed;

--- a/run.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/run.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -29,13 +29,6 @@
     magref.z /= float(n_init);
     palt0 /= float(n_init);
     twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
-    //センサ正常性チェック
-    //usaPack通信開始
-    pc.Subscribe(0000, &(vp));
-
-    //制御ループのアタッチ
-    LoopTicker PIDtick;
-    PIDtick.attach(calcServoOut,PID_dt);
     
     //ESKFの共分散設定
     eskf.setGravity(0.0f,0.0f,9.8f);
@@ -46,22 +39,84 @@
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
     
-    eskf.setQVelocity(0.000025f);
-    eskf.setQAngleError(0.00025f);
-    eskf.setQAccBias(0.0000001f);
+    eskf.setQVelocity(0.001f);
+    eskf.setQAngleError(0.0000025f);
+    eskf.setQAccBias(0.000001f);
     eskf.setQGyroBias(0.000001f);
     
+    Matrix Rgpspos(3,3);
+    setDiag(Rgpspos,1.0f);
+    
+    Matrix Rgpsvel(3,3);
+    Rgpsvel(1,1) = 0.01f;
+    Rgpsvel(2,2) = 0.01f;
+    Rgpsvel(3,3) = 0.1f;
+    
     Matrix Rgps(6,6);
-    setDiag(Rgps,1.0f);
-    //Rgps(4,4) = 0.1f;
-    //Rgps(5,5) = 0.1f;
+    setDiag(Rgps,0.05f);
+    Rgps(4,4) = 0.1f;
+    Rgps(5,5) = 0.1f;
     
+    float dynAccCriteria = 0.4f;
     Matrix Racc(3,3);
-    setDiag(Racc,50.0f);
+    setDiag(Racc,100.0f);
+    Matrix RaccDyn(3,3);
+    setDiag(RaccDyn,5000.0f);
 
     Matrix Rheading(1,1);
     Rheading(1,1) = 0.01f;
+    
     _t.start();
+    //センサ正常性チェック
+    if(hilFlag == false){
+        while(1){
+            float tstart = _t.read();
+            getIMUval();
+            getGPSval();
+            eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+            eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
+            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+            float heading = atan2f(-mag.y,mag.x);
+            eskf.updateHeading(heading,Rheading);
+            Matrix Raccpreflight(3,3);
+            setDiag(Raccpreflight,5.0f);
+            eskf.updateAcc(MatrixMath::Vector2mat(acc),Raccpreflight);
+            
+            bool preflightCheck = true;
+            Matrix gyroBias = eskf.getGyroBias();
+            if(fabsf(gyro.x-gyroBias(1,1))>2.0f || fabsf(gyro.y-gyroBias(2,1))>2.0f || fabsf(gyro.z-gyroBias(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high gyro value\r\n");
+            }
+            Matrix accBias = eskf.getAccBias();
+            if(fabsf(acc.x-accBias(1,1))>2.0f || fabsf(acc.y-gyroBias(2,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high acc value\r\n");
+            }
+            Matrix vihat = eskf.getVihat();
+            if(fabsf(vihat(1,1))>2.0f || fabsf(vihat(2,1))>2.0f||fabsf(vihat(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : high velocity value\r\n");
+            }
+            Matrix pihat = eskf.getPihat();
+            if(fabsf(pihat(1,1))>2.0f || fabsf(pihat(2,1))>2.0f||fabsf(pihat(3,1))>2.0f){
+                preflightCheck = false;
+                twelite.printf("PreFlight Check : not home position\r\n");
+            }
+            
+            if(preflightCheck == true){
+                break;
+            }
+        }
+    }
+    twelite.printf("PreFlight Check Completed\r\n");
+    //usaPack通信開始
+    pc.Subscribe(0000, &(vp));
+
+    //制御ループのアタッチ
+    LoopTicker PIDtick;
+    PIDtick.attach(calcServoOut,PID_dt);
+    
     float tgps = _t.read();
     float theading = _t.read();
     while(1)
@@ -86,6 +141,10 @@
                 gpsUpdateFlag = false;
             }
         }else{
+            if(userButton.read()==1)
+            {
+                gpsLlh0Fixed = false;
+            };  
             getGPSval();
         }
         
@@ -100,12 +159,20 @@
         
         if(gpsUpdateFlag == true){
             eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
-            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgps);
+            //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
+            //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel);
         }else if(headingUpdateFlag == true){
                 float heading = atan2f(-mag.y,mag.x);
                 eskf.updateHeading(heading,Rheading);
         }else{
-            eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            Matrix dynacc = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
+            dynaccnorm2 = dynacc(1,1)*dynacc(1,1)+dynacc(2,1)*dynacc(2,1)+dynacc(3,1)*dynacc(3,1);
+            //twelite.printf("%f\r\n",sqrt(dynaccnorm2));
+            if(dynaccnorm2 > dynAccCriteria*dynAccCriteria){
+                eskf.updateAcc(MatrixMath::Vector2mat(acc),RaccDyn);
+            }else{
+                eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc);
+            }
         }
         PIDtick.loop(); 
         

--- a/servo.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/servo.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -31,6 +31,9 @@
         rollPID.setSetPoint(roll_obj);
         pitchPID.setSetPoint(pitch_obj);
         dT += dT_obj;
+    }else{
+        rollPID.setSetPoint(0.0f);
+        pitchPID.setSetPoint(0.0f);
     }
     
     //舵角計算

--- a/setup.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/setup.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -51,8 +51,8 @@
     servoLeft.pulsewidth_us(1500.0); 
     servoThrust.pulsewidth_us(1100.0);
     
-    autopilot.set_dest(0.0f, 0.0f);
-    autopilot.set_turn(0.0f, 0.0f, 50.0f);
+    autopilot.set_dest(0.0f, 50.0f);
+    autopilot.set_turn(0.0f, 50.0f, 50.0f);
     autopilot.set_alt(30.0f, 10.0f);
 }
 

--- a/solaESKF.lib	Mon Nov 22 09:51:51 2021 +0000
+++ b/solaESKF.lib	Mon Nov 29 09:45:44 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/solaESKF/#f5fe7fecbd3c
+https://os.mbed.com/teams/HAPSRG/code/solaESKF/#e2c825cdc511

--- a/transferData.cpp	Mon Nov 22 09:51:51 2021 +0000
+++ b/transferData.cpp	Mon Nov 29 09:45:44 2021 +0000
@@ -28,7 +28,9 @@
     
     //twelite.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
     //twelite.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1),gravity(1,1),gravity(2,1),gravity(3,1));
-    twelite.printf("%d %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));
+        
+    twelite.printf("%d %f %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,_t.read(),rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1),sqrt(dynaccnorm2));
+    //twelite.printf("%d %f %f %f %f %f %f %f %f %f %f\r\n",gps.gpsFix ,1.0f/att_dt,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI,pi.x,pi.y,pi.z,vi.x,vi.y,vi.z);
     //twelite.printf("%f %f %f %f %f %f\r\n",accBias(1,1),accBias(2,1),accBias(3,1),gyroBias(1,1),gyroBias(2,1),gyroBias(3,1));
     //twelite.printf("%f %f %f %f %f %f\r\n",pi.x,pi.y,pi.z,vi.x,vi.y,vi.z);
     //twelite.printf("%f %f %f %f %f %f %f \r\n", magGamma(7,1), magGamma(8,1), magGamma(9,1), mag.x, mag.y, mag.z,magres);
@@ -42,7 +44,7 @@
     Matrix gyroBias = eskf.getGyroBias();
     Matrix gravity = eskf.getGravity();
     //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
-    sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),1.0f/att_dt,da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));
+    sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",_t.read(),da,de,dT,rc[0],rc[1],rc[2],rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI, pihat(1,1),pihat(2,1),pihat(3,1),vihat(1,1),vihat(2,1),vihat(3,1));
 
     //sd.printf("%f %f %f %f %f %f\r\n",da,de,dT,rpy.x*180.0f/M_PI,rpy.y*180.0f/M_PI,rpy.z*180.0f/M_PI);
 }