solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
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);