Diff: main.cpp

Revision:

53:3a6dfb55e087

Parent:

52:abec61ea3cd3

Child:

54:ca88777b295a

--- a/main.cpp	Mon May 24 03:39:49 2021 +0000
+++ b/main.cpp	Mon May 24 05:45:56 2021 +0000
@@ -9,6 +9,7 @@
 #include "Matrix.h"
 #include "MatrixMath.h"
 #include "math.h"
+#include "UsaPack.hpp"
 
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
@@ -25,12 +26,12 @@
 #define motorPwmMax  2000.0
 #define motorPwmMin  1100.0
 
-#define N_EEPROM 10
+#define N_EEPROM 11
 
 MPU6050 accelgyro;
 MAG3110 mag(PB_9,PB_8);
 SBUS sbus(PD_5, PD_6);
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX, 115200);
 DigitalIn userButton(USER_BUTTON);
 DigitalIn locdef1(PG_2);
 DigitalIn locdef2(PG_3);
@@ -110,20 +111,57 @@
  char c[N_EEPROM*4]; // floatはcharの4倍
 };
 
+// UsaPack
+UsaPack tail(PG_14, PG_9, 115200);
+int tail_address = 0;
+struct valuePack
+{
+    float dt;
+    int count;
+    float acc[3];
+    float gyro[3];
+    float mag[3];
+    float rot[3];
+    float rot_g[3];
+};
+valuePack vp;
+
+
 void writeSdcard()
 {
     //magcal.getExtremes(&magmin[0],&magmax[0]);
     //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
     //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
-    //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+//    sd.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
-    //pc.printf("%f %f \r\n",rc[1],servoOut[0]);
-    pc.printf("%f %f \r\n",roll*180.0f/pi,pitch*180.0f/pi);
-    //pc.printf("%f %f\r\n",roll_g*180.0f/pi,pitch_g*180.0f/pi);
+    //pc.printf("%d \r\n",userButton.read());
+    //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
+    //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
     //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
     //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
     //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
+    
+    vp.dt = 1.0f/att_dt;
+    vp.count = obs_count;
+    vp.acc[0] = acc_x;
+    vp.acc[1] = acc_y;
+    vp.acc[2] = acc_z;
+    vp.gyro[0] = gyro_x;
+    vp.gyro[1] = gyro_y;
+    vp.gyro[2] = gyro_z;
+    vp.mag[0] = mag_x;
+    vp.mag[1] = mag_y;
+    vp.mag[2] = mag_z;
+    vp.rot[0] = roll*180.0f/pi;
+    vp.rot[1] = pitch*180.0f/pi;
+    vp.rot[2] = yaw*180.0f/pi;
+    vp.rot_g[0] = roll_g*180.0f/pi;
+    vp.rot_g[1] = pitch_g*180.0f/pi;
+    vp.rot_g[2] = yaw_g*180.0f/pi;
+    tail.Send(tail_address, &vp);
+//    pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+    
 }
 
 // eeprom書き込み・読み込みに必要な関数
@@ -498,7 +536,7 @@
     servo.period_us(15000.0);
     servo.pulsewidth_us(1500.0);
     
-    pc.baud(57600);
+//    pc.baud(57600);
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
     accelgyro.setFullScaleAccelRange(ACCEL_FSR);
@@ -550,6 +588,7 @@
         magbias[3] = float(read_calib.i[4])/1000.0;
         pitch_align = float(read_calib.i[8])/200000.0;
         roll_align = float(read_calib.i[9])/200000.0;
+        tail_address = (int)read_calib.i[10];
         
         switch(pos_tail){
             case 1:
@@ -565,6 +604,7 @@
                 pc.printf("error\r\n");
                 break;
         }
+        pc.printf("tail_address : %d\r\n", tail_address);
         pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",pitch_align*180.0/pi,roll_align*180.0/pi);
         getIMUval();
         triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
@@ -648,21 +688,25 @@
             wait(2);
             if(userButton.read() == 0){
                 pos_tail=1;
+                tail_address = 1111;
                 break;
             };
             pc.printf("Center\r\n");
             wait(2);
             if(userButton.read() == 0){
                 pos_tail=2;
+                tail_address = 2222;
                 break;
             };
             pc.printf("Right\r\n");
             wait(2);
             if(userButton.read() == 0){
                 pos_tail=3;
+                tail_address = 3333;
                 break;
             };
         };
+        pc.printf("tail_address : %d\r\n", tail_address);
         switch(pos_tail){
             case 1:
                 pc.printf("This MBED is Located at Left \r\n");
@@ -734,8 +778,9 @@
           transfer_data.i[i] = 0;
         }
         }
+        transfer_data.i[10] = tail_address;
         for (int i = 0; i < N_EEPROM; i++) {
-        pc.printf("transfer_data[%d]: %\d\r\n", i, transfer_data.i[i]);
+        pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]);
         }
         writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4);
         wait(3);