Revision 140:53dbdb207542, committed 2021-12-06

Comitter:

cocorlow

Date:

Mon Dec 06 11:37:55 2021 +0000

Parent:

139:b378528c05f2

Child:

141:725321fe2949

Commit message:

servo transferData imu

Changed in this revision

--- a/Autopilot.lib	Mon Dec 06 08:26:16 2021 +0000
+++ b/Autopilot.lib	Mon Dec 06 11:37:55 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/Autopilot_Eigen/#28f480b41553
+https://os.mbed.com/teams/HAPSRG/code/Autopilot_Eigen/#36d034dec6d6

--- a/autopilot.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/autopilot.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,52 +1,53 @@
-//#include "global.hpp"
-// 
-//void level_flight()
-//{
-//    Vector3 vdot = calc_vdot();
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-//    autopilot.level();
-//    autopilot.keep_alt();
-//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-//}
-// 
-//void point_guide()
-//{
-//    Vector3 vdot = calc_vdot();
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-//    autopilot.guide();
-//    autopilot.keep_alt();
-//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
-//}
-// 
-//void turning()
-//{
-//    Vector3 vdot = calc_vdot();
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-//    autopilot.turn();
-//    autopilot.keep_alt();
-//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-//}
-//
-//void climb()
-//{
-//    Vector3 vdot = calc_vdot();
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
-//    autopilot.level();
-//    autopilot.climb();
-//    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
-//}
-//
-//Vector3 calc_vdot()
-//{
-//    Matrix m_vdot = eskf.calcDynAcc(MatrixMath::Vector2mat(acc));
-//    Vector3 vdot(m_vdot(1, 1), m_vdot(2, 1), m_vdot(3, 1));
-//    return vdot;
-//}
\ No newline at end of file
+#include "global.hpp"
+ 
+void level_flight()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.level();
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+ 
+void point_guide()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.guide();
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
+}
+ 
+void turning()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.turn();
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+
+void climb()
+{
+    Vector3f vdot = calc_vdot();
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    autopilot.update_val(rpy, -palt, pihat, vihat, vdot);
+    autopilot.level();
+    autopilot.climb();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
+}
+
+Vector3f calc_vdot()
+{
+    Vector3f m_vdot = eskf.calcDynAcc(acc);
+    Vector3f vdot;
+    vdot = m_vdot;
+    return vdot;
+}
\ No newline at end of file

--- a/global.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/global.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -25,9 +25,9 @@
 PID pitchratePID(1.0f, 0.0f, 0.0f, PID_dt);//rad/s
 PID rollPID(5.0f,0.0f,0.0f,PID_dt);
 PID rollratePID(0.05f, 0.0, 0.0, PID_dt);//rad/s
-//solaESKF eskf; // ESKF class
+solaESKF eskf; // ESKF class
 int obsCount = 0;
-//Autopilot autopilot;
+Autopilot autopilot;
 float roll_obj;
 float pitch_obj;
 float dT_obj;
@@ -36,9 +36,9 @@
 int loop_count = 0;
 float att_dt = 0.01f;
 // position
-Matrix3f SensorAlignmentAG(3,3);
-Matrix3f SensorAlignmentMAG(3,3);
-Vector3f euler(3,1);
+Matrix3f SensorAlignmentAG;
+Matrix3f SensorAlignmentMAG;
+Vector3f euler;
 Vector3f rpy(0.0f, 0.0f, 0.0f); // x:roll  y:pitch  z:yaw
 Vector3f acc;
 Vector3f accref(0.0f, 0.0f, 9.8f);

--- a/global.hpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/global.hpp	Mon Dec 06 11:37:55 2021 +0000
@@ -112,9 +112,9 @@
 extern PID pitchratePID;//rad/s
 extern PID rollPID;
 extern PID rollratePID;//rad/s
-//extern solaESKF eskf; // EKF class
+extern solaESKF eskf; // EKF class
 extern int obsCount;
-//extern Autopilot autopilot;
+extern Autopilot autopilot;
 extern float roll_obj;
 extern float pitch_obj;
 extern float dT_obj;

--- a/imu.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/imu.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,47 +1,43 @@
-//#include "global.hpp"
-//
-//void getIMUval()
-//{
-//        lsm.readAccel();
-//        lsm.readMag();
-//        lsm.readGyro();
-//        
-//        Matrix accmat(3,1);
-//        accmat << lsm.ax * 9.8f - agoffset[0] << lsm.ay * 9.8f - agoffset[1] << lsm.az * 9.8f - agoffset[2];
-//        Matrix accAlign = SensorAlignmentAG*accmat;
-//        
-//        acc.x = accAlign(1,1);
-//        acc.y = accAlign(2,1);
-//        acc.z = accAlign(3,1);
-//        
-//        Matrix gyromat(3,1);
-//        gyromat << (lsm.gx * M_PI / 180.0f) - agoffset[3] << (lsm.gy * M_PI / 180.0f) - agoffset[4] << (lsm.gz * M_PI / 180.0f) - agoffset[5];
-//        Matrix gyroAlign = SensorAlignmentAG*gyromat;
-//        gyro.x = gyroAlign(1,1);
-//        gyro.y = gyroAlign(2,1);
-//        gyro.z = gyroAlign(3,1);
-//        
-//        Matrix magraw(3,1);
-//        magraw << lsm.mx <<lsm.my << lsm.mz;
-//        magraw = SensorAlignmentMAG*magraw;
-//        float inputMag[3];
-//        float outputMag[3];
-//        inputMag[0] = magraw(1,1)*1000.0f;
-//        inputMag[1] = magraw(2,1)*1000.0f;
-//        inputMag[2] = magraw(3,1)*1000.0f;
-//        magCalibrator.run(inputMag,outputMag);
-//        mag.x = outputMag[0];
-//        mag.y = outputMag[1];
-//        mag.z = outputMag[2];
-//        //twelite.printf("%f %f %f : %f %f %f\r\n",magraw(1,1),magraw(2,1),magraw(3,1),magmod(1,1),magmod(2,1),magmod(3,1));
-//        
-//        palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
-//        
-//        //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
-//        //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
-//        //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
-//        //float pressure = lps.readPressureMillibars();
-//        //float altitude = lps.pressureToAltitudeMeters(pressure);
-//        //float temperature = lps.readTemperatureC();
-//        //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
-//}   
\ No newline at end of file
+#include "global.hpp"
+
+void getIMUval()
+{
+        lsm.readAccel();
+        lsm.readMag();
+        lsm.readGyro();
+        
+        Vector3f accmat;
+        accmat << lsm.ax * 9.8f - agoffset[0], lsm.ay * 9.8f - agoffset[1], lsm.az * 9.8f - agoffset[2];
+        Vector3f accAlign = SensorAlignmentAG*accmat;
+        
+        acc = accAlign;
+        
+        Vector3f gyromat;
+        gyromat << (lsm.gx * M_PI_F / 180.0f) - agoffset[3], (lsm.gy * M_PI_F / 180.0f) - agoffset[4], (lsm.gz * M_PI_F / 180.0f) - agoffset[5];
+        Vector3f gyroAlign = SensorAlignmentAG*gyromat;
+        gyro = gyroAlign;
+        
+        Vector3f magraw;
+        magraw << lsm.mx, lsm.my, lsm.mz;
+        magraw = SensorAlignmentMAG*magraw;
+        float inputMag[3];
+        float outputMag[3];
+        inputMag[0] = magraw(0)*1000.0f;
+        inputMag[1] = magraw(1)*1000.0f;
+        inputMag[2] = magraw(2)*1000.0f;
+        magCalibrator.run(inputMag,outputMag);
+        mag(0) = outputMag[0];
+        mag(1) = outputMag[1];
+        mag(2) = outputMag[2];
+        //twelite.printf("%f %f %f : %f %f %f\r\n",magraw(1,1),magraw(2,1),magraw(3,1),magmod(1,1),magmod(2,1),magmod(3,1));
+        
+        palt = -(lps.pressureToAltitudeMeters(lps.readPressureMillibars())-palt0);
+        
+        //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
+        //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
+        //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
+        //float pressure = lps.readPressureMillibars();
+        //float altitude = lps.pressureToAltitudeMeters(pressure);
+        //float temperature = lps.readTemperatureC();
+        //twelite.printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
+}   
\ No newline at end of file

--- a/servo.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/servo.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,93 +1,91 @@
-//#include "global.hpp"
-//
-//// 割り込まれた時点での出力(computeの結果）を返す関数
-//void calcServoOut()
-//{
-//    // sbusデータの読み込み
-//    for (int i =0 ; i < 16;i ++){
-//        rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
-//    }
-//
-//    
-//    //姿勢角の所得
-//    euler = eskf.computeAngles();
-//    rpy.x = euler(1,1);
-//    rpy.y = euler(2,1);
-//    rpy.z = euler(3,1);
-//
-//    //PIDへの状態量のセット
-//    pitchPID.setProcessValue(rpy.y);
-//    pitchratePID.setProcessValue(gyro.y);
-//    rollPID.setProcessValue(rpy.x);
-//    rollratePID.setProcessValue(gyro.x);
-//    
-//    dT = rc[2];
-//    
-//    if (rc[4]>-0.3f && rc[6] < -0.3f)
-//    {
-//        //level_flight();
-//        //point_guide();
-//        climb();
-//        rollPID.setSetPoint(roll_obj);
-//        pitchPID.setSetPoint(pitch_obj);
-//        dT += dT_obj;
-//    }else{
-//        rollPID.setSetPoint(0.0f);
-//        pitchPID.setSetPoint(0.0f);
-//    }
-//    
-//    //舵角計算
-//    if(rc[4]<-0.3f){
-//        de = (rc[0]-rc[1])/2.0f;
-//        da = (rc[0]+rc[1])/2.0f;
-//    }else{
-//        de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
-//        da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
-//    }
-//    
-//    scaledServoOut[0]=de+da;
-//    scaledServoOut[1]=-de+da;
-//    scaledMotorOut[0]= dT;
-//    
-//    float LP_servo = 0.2;
-//    float LP_motor = 0.2;
-//    for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
-//    {
-//        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
-//        if(servoOut[i]<servoPwmMin)
-//        {
-//            servoOut[i] = servoPwmMin;
-//        }
-//        if(servoOut[i]>servoPwmMax)
-//        {
-//            servoOut[i] = servoPwmMax;
-//        }
-//    }
-//    
-//    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
-//        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
-//        if(motorOut[i]<motorPwmMin) {
-//            motorOut[i] = motorPwmMin;
-//        };
-//        if(motorOut[i]>motorPwmMax) {
-//            motorOut[i] = motorPwmMax;
-//        };
-//    }
-//    servoRight.pulsewidth_us(servoOut[0]);
-//    servoLeft.pulsewidth_us(servoOut[1]); 
-//    servoThrust.pulsewidth_us(motorOut[0]);
-//
-//    sendData2PC();
-//    writeSDcard();
-//    
-//    if(loop_count >= 5)
-//    {
-//        sendTelemetry();
-//        loop_count = 1;
-//
-//    }
-//    else
-//    {
-//        loop_count +=1;
-//    }
-//}
\ No newline at end of file
+#include "global.hpp"
+
+// 割り込まれた時点での出力(computeの結果）を返す関数
+void calcServoOut()
+{
+    // sbusデータの読み込み
+    for (int i =0 ; i < 16;i ++){
+        rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
+    }
+
+    
+    //姿勢角の所得
+    euler = eskf.computeAngles();
+    rpy = euler;
+
+    //PIDへの状態量のセット
+    pitchPID.setProcessValue(rpy(1));
+    pitchratePID.setProcessValue(gyro(1));
+    rollPID.setProcessValue(rpy(0));
+    rollratePID.setProcessValue(gyro(0));
+    
+    dT = rc[2];
+    
+    if (rc[4]>-0.3f && rc[6] < -0.3f)
+    {
+        //level_flight();
+        //point_guide();
+        climb();
+        rollPID.setSetPoint(roll_obj);
+        pitchPID.setSetPoint(pitch_obj);
+        dT += dT_obj;
+    }else{
+        rollPID.setSetPoint(0.0f);
+        pitchPID.setSetPoint(0.0f);
+    }
+    
+    //舵角計算
+    if(rc[4]<-0.3f){
+        de = (rc[0]-rc[1])/2.0f;
+        da = (rc[0]+rc[1])/2.0f;
+    }else{
+        de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
+        da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
+    }
+    
+    scaledServoOut[0]=de+da;
+    scaledServoOut[1]=-de+da;
+    scaledMotorOut[0]= dT;
+    
+    float LP_servo = 0.2;
+    float LP_motor = 0.2;
+    for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
+    {
+        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1.0f,1.0f,servoPwmMin,servoPwmMax))+(1.0f-LP_servo)*servoOut[i];
+        if(servoOut[i]<servoPwmMin)
+        {
+            servoOut[i] = servoPwmMin;
+        }
+        if(servoOut[i]>servoPwmMax)
+        {
+            servoOut[i] = servoPwmMax;
+        }
+    }
+    
+    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
+        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1.0f,1.0f,motorPwmMin,motorPwmMax))+(1.0f-LP_motor)*motorOut[i];
+        if(motorOut[i]<motorPwmMin) {
+            motorOut[i] = motorPwmMin;
+        };
+        if(motorOut[i]>motorPwmMax) {
+            motorOut[i] = motorPwmMax;
+        };
+    }
+    servoRight.pulsewidth_us(servoOut[0]);
+    servoLeft.pulsewidth_us(servoOut[1]); 
+    servoThrust.pulsewidth_us(motorOut[0]);
+
+    sendData2PC();
+    writeSDcard();
+    
+    if(loop_count >= 5)
+    {
+        sendTelemetry();
+        loop_count = 1;
+
+    }
+    else
+    {
+        loop_count +=1;
+    }
+}
\ No newline at end of file

--- a/transferData.cpp	Mon Dec 06 08:26:16 2021 +0000
+++ b/transferData.cpp	Mon Dec 06 11:37:55 2021 +0000
@@ -1,76 +1,76 @@
-//#include "global.hpp"
-//
-//void sendData2PC()
-//{
-//    sp.da = da;
-//    sp.de = de;
-//    sp.dT = dT;
-//    sp.rpy[0] = rpy.x*180.0f/M_PI;
-//    sp.rpy[1] = rpy.y*180.0f/M_PI;
-//    sp.rpy[2] = rpy.z*180.0f/M_PI;
-//    Matrix vihat = eskf.getVihat();
-//    sp.vihat[0] = vihat(1,1);
-//    sp.vihat[1] = vihat(2,1);
-//    sp.vihat[2] = vihat(3,1);
-//    pc.Send(0000, &(sp));
-//}
-//
-//void sendTelemetry()
-//{
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//    tp.time=_t.read();
-//    tp.hertz = 1.0f/att_dt;
-//    tp.gpsFix = float(gps.gpsFix);
-//    for(int i = 0;i<3;i++){
-//        tp.rpy[i] = euler(i+1,1)*180.0f/M_PI;
-//        tp.pihat[i] = pihat(i+1,1);
-//        tp.vihat[i] = vihat(i+1,1);
-//    }
-//    tp.dynaccNorm = sqrt(dynaccnorm2);
-//    
-//    twelite.Send(0000, &(tp));
-//        
-//}
-//
-//void writeSDcard()
-//{
-//    Matrix pihat = eskf.getPihat();
-//    Matrix vihat = eskf.getVihat();
-//
-//    lp.time = _t.read();
-//    lp.hertz = 1.0f/att_dt;
-//    lp.gpsFix = float(gps.gpsFix);
-//    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);
-//}
+#include "global.hpp"
+
+void sendData2PC()
+{
+    sp.da = da;
+    sp.de = de;
+    sp.dT = dT;
+    sp.rpy[0] = rpy(0)*180.0f/M_PI_F;
+    sp.rpy[1] = rpy(1)*180.0f/M_PI_F;
+    sp.rpy[2] = rpy(2)*180.0f/M_PI_F;
+    Vector3f vihat = eskf.getVihat();
+    sp.vihat[0] = vihat(0);
+    sp.vihat[1] = vihat(1);
+    sp.vihat[2] = vihat(2);
+    pc.Send(0000, &(sp));
+}
+
+void sendTelemetry()
+{
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+    tp.time=_t.read();
+    tp.hertz = 1.0f/att_dt;
+    tp.gpsFix = float(gps.gpsFix);
+    for(int i = 0;i<3;i++){
+        tp.rpy[i] = euler(i*180.0f/M_PI_F);
+        tp.pihat[i] = pihat(i);
+        tp.vihat[i] = vihat(i);
+    }
+    tp.dynaccNorm = std::sqrt(dynaccnorm2);
+    
+    twelite.Send(0000, &(tp));
+        
+}
+
+void writeSDcard()
+{
+    Vector3f pihat = eskf.getPihat();
+    Vector3f vihat = eskf.getVihat();
+
+    lp.time = _t.read();
+    lp.hertz = 1.0f/att_dt;
+    lp.gpsFix = float(gps.gpsFix);
+    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);
+        lp.pihat[i] = pihat(i);
+        lp.vihat[i] = vihat(i);
+    }
+    lp.pi[0] = pi(0);
+    lp.pi[1] = pi(1);
+    lp.pi[2] = pi(2);
+    lp.vi[0] = vi(0);
+    lp.vi[1] = vi(1);
+    lp.vi[2] = vi(2);
+    lp.acc[0] = acc(0);
+    lp.acc[1] = acc(1);
+    lp.acc[2] = acc(2);
+    lp.gyro[0] = gyro(0);
+    lp.gyro[1] = gyro(1);
+    lp.gyro[2] = gyro(2);
+    lp.mag[0] = mag(0);
+    lp.mag[1] = mag(1);
+    lp.mag[2] = mag(2);
+    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);
+}