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 133:346ce20b3950, committed 2021-11-30

Comitter:
NaotoMorita
Date:
Tue Nov 30 07:58:46 2021 +0000
Parent:
131:45542fcc886f
Parent:
132:896ad37b534b
Child:
134:d57c6b2a706b
Commit message:
log usapack

Changed in this revision

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
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
transferData.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/global.cpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/global.cpp	Tue Nov 30 07:58:46 2021 +0000
@@ -5,8 +5,8 @@
 
 // communication
 UsaPack pc(USBTX, USBRX, 115200); // log - tail
-Serial sd(PG_14,PG_9);
-Serial twelite(PD_1,PD_0);
+UsaPack sd(PG_14,PG_9, 115200);
+UsaPack twelite(PD_1,PD_0);
 // io
 DigitalIn userButton(USER_BUTTON);
 SBUS sbus(PD_5, PD_6);
@@ -37,7 +37,8 @@
 float att_dt = 0.01f;
 // position
 Matrix SensorAlignmentAG(3,3);
-Matrix SensorAlignmentMAG(3,3); 
+Matrix SensorAlignmentMAG(3,3);
+Matrix euler(3,1);
 Vector3 rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3 acc;
 Vector3 accref(0.0f, 0.0f, 9.8f);
@@ -81,6 +82,7 @@
 // UsaPack
 valuePack vp;
 sendPack sp;
+logPack lp;
 
 // HIL
 bool hilFlag = true;

--- a/global.hpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/global.hpp	Tue Nov 30 07:58:46 2021 +0000
@@ -50,14 +50,32 @@
     float rpy[3];
     float vihat[3];
 };
+struct logPack
+{
+    float time;
+    float hertz;
+    float da;
+    float de;
+    float dT;
+    float rpy[3];
+    float pihat[3];
+    float vihat[3];
+    float pi[3];
+    float vi[3];
+    float palt;
+    float acc[3];
+    float gyro[3];
+    float mag[3];
+    float rc[16];
+};
 
 // var
 
 // communication
 
 extern UsaPack pc; // log - tail
-extern Serial sd;
-extern Serial twelite;
+extern UsaPack sd;
+extern UsaPack twelite;
 
 // io
 extern DigitalIn userButton;
@@ -93,6 +111,7 @@
 // position
 extern Matrix SensorAlignmentAG;
 extern Matrix SensorAlignmentMAG; 
+extern Matrix euler;
 extern Vector3 rpy; // x:roll  y:pitch  z:yaw
 extern Vector3 acc;
 extern Vector3 accref;
@@ -136,6 +155,7 @@
 //// UsaPack
 extern valuePack vp;
 extern sendPack sp;
+extern logPack lp;
 
 // HIL
 extern bool hilFlag;

--- a/servo.cpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/servo.cpp	Tue Nov 30 07:58:46 2021 +0000
@@ -10,7 +10,7 @@
 
     
     //姿勢角の所得
-    Matrix euler = eskf.computeAngles();
+    euler = eskf.computeAngles();
     rpy.x = euler(1,1);
     rpy.y = euler(2,1);
     rpy.z = euler(3,1);
@@ -78,10 +78,10 @@
     servoThrust.pulsewidth_us(motorOut[0]);
 
     sendData2PC();
-
+    writeSDcard();
+    
     if(loop_count >= 5)
     {
-        writeSDcard();
         sendTelemetry();
         loop_count = 1;
 

--- a/setup.cpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/setup.cpp	Tue Nov 30 07:58:46 2021 +0000
@@ -3,9 +3,9 @@
 
 void setup()
 {
-    sd.baud(115200);
-    sd.printf("\r\nFlight Start\r\n");
-    twelite.baud(38400);
+    //sd.baud(115200);
+    //sd.printf("\r\nFlight Start\r\n");
+    //twelite.baud(38400);
     twelite.printf("\r\nTelemetory Start\r\n");
     uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
     //printf("%x\n", reg);

--- a/solaESKF.lib	Mon Nov 29 13:13:34 2021 +0000
+++ b/solaESKF.lib	Tue Nov 30 07:58:46 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/solaESKF/#e2c825cdc511
+https://os.mbed.com/teams/HAPSRG/code/solaESKF/#cd1b21936ef1

--- a/transferData.cpp	Mon Nov 29 13:13:34 2021 +0000
+++ b/transferData.cpp	Tue Nov 30 07:58:46 2021 +0000
@@ -22,14 +22,15 @@
     Matrix accBias = eskf.getAccBias();
     Matrix gyroBias = eskf.getGyroBias();
     Matrix gravity = eskf.getGravity();
-    magCalibrator.getExtremes(magbiasMin,magbiasMax);
+    //magCalibrator.getExtremes(magbiasMin,magbiasMax);
     //twelite.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]);
     //twelite.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]);
     
     //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.Send(0000, &(sp));
         
-    twelite.printf("%d %f %f %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),gps.Longitude,gps.Latitude);
+    //twelite.printf("%d %f %f %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),gps.Longitude,gps.Latitude);
     //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);
@@ -40,11 +41,39 @@
 {
     Matrix pihat = eskf.getPihat();
     Matrix vihat = eskf.getVihat();
-    Matrix accBias = eskf.getAccBias();
-    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\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));
+
+    lp.time = _t.read();
+    lp.hertz = 1.0f/att_dt;
+    lp.da = da;
+    lp.de = de;
+    lp.dT = dT;
+    for(int i = 0;i<16;i++){
+        lp.rc[i] = rc[i];
+    }
+    for(int i = 0;i<3;i++){
+        lp.rpy[i] = euler(i+1,1);
+        lp.pihat[i] = pihat(i+1,1);
+        lp.vihat[i] = vihat(i+1,1);
+    }
+    lp.pi[0] = pi.x;
+    lp.pi[1] = pi.y;
+    lp.pi[2] = pi.z;
+    lp.vi[0] = vi.x;
+    lp.vi[1] = vi.y;
+    lp.vi[2] = vi.z;
+    lp.acc[0] = acc.x;
+    lp.acc[1] = acc.y;
+    lp.acc[2] = acc.z;
+    lp.gyro[0] = gyro.x;
+    lp.gyro[1] = gyro.y;
+    lp.gyro[2] = gyro.z;
+    lp.mag[0] = mag.x;
+    lp.mag[1] = mag.y;
+    lp.mag[2] = mag.z;
+    lp.palt = palt;
 
     //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\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.Send(0000, &(lp));
+    //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);
 }