HAPSRG
Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 52:abec61ea3cd3
- Parent:
- 49:73fe59148dd4
- Child:
- 53:3a6dfb55e087
--- a/main.cpp Fri May 21 01:35:05 2021 +0000
+++ b/main.cpp Mon May 24 03:39:49 2021 +0000
@@ -25,7 +25,7 @@
#define motorPwmMax 2000.0
#define motorPwmMin 1100.0
-#define N_EEPROM 8
+#define N_EEPROM 10
MPU6050 accelgyro;
MAG3110 mag(PB_9,PB_8);
@@ -98,8 +98,8 @@
int pos_tail = 1;//1:left 2:center 3:right
int agoffset[6] = {0, 0, 0, 386, -450, 48};
float magbias[4] = {-215.833374, 207.740616, -167.444565, 36.688690};
-float pitch_align = 0.0*3.141562/180.0
-float roll_align = 0.0*3.141562/180.0
+float pitch_align = 0.0*3.141562/180.0;
+float roll_align = 0.0*3.141562/180.0;
// eepromのread writeのためのunionを定義
int address = 0xAE; // EEPROMの3つの入力が全て+より
int pointeraddress = 0;
@@ -119,7 +119,7 @@
//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 %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+ 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("%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);
@@ -432,7 +432,8 @@
rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
}
}
-
+ pitchPID.setGain(6.36, 10.6,0.0);
+ pitchratePID.setGain(0.9540,0.0,0.0);
pitchPID.setProcessValue(pitch);
pitchratePID.setProcessValue(gyro_y);
float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
@@ -514,18 +515,18 @@
D.add(2,2,1.0);
D.add(3,3,1.0);
- Phat.add(1,1,0.1);
- Phat.add(2,2,0.1);
- Phat.add(3,3,0.1);
- Phat.add(4,4,0.1);
+ Phat.add(1,1,0.05);
+ Phat.add(2,2,0.05);
+ Phat.add(3,3,0.05);
+ Phat.add(4,4,0.05);
Qgyro.add(1,1,0.0224);
Qgyro.add(2,2,0.0224);
Qgyro.add(3,3,0.0224);
- Racc.add(1,1,0.0330*100);
- Racc.add(2,2,0.0330*100);
- Racc.add(3,3,0.0330*100);
+ Racc.add(1,1,0.0330*200);
+ Racc.add(2,2,0.0330*200);
+ Racc.add(3,3,0.0330*200);
Rmag.add(1,1,1.0);
Rmag.add(2,2,1.0);
@@ -537,9 +538,8 @@
pc.printf("reading calibration value\r\n");
//キャリブレーション値を取得
U read_calib;
- readEEPROM(address, pointeraddress, read_calib.c, 32);
+ readEEPROM(address, pointeraddress, read_calib.c, N_EEPROM*4);
wait(3);
- for (int i = 0 ; i < N_EEPROM; i ++){
pos_tail = read_calib.i[0];
agoffset[3] = float(read_calib.i[5]);
agoffset[4] = float(read_calib.i[6]);
@@ -548,7 +548,9 @@
magbias[1] = float(read_calib.i[2])/1000.0;
magbias[2] = float(read_calib.i[3])/1000.0;
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;
+
switch(pos_tail){
case 1:
pc.printf("This MBED is Located at Left \r\n");
@@ -563,6 +565,7 @@
pc.printf("error\r\n");
break;
}
+ 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);
float sumLPaccnorm = 0;
@@ -675,7 +678,29 @@
break;
}
+ //姿勢オフセットを計算
+ pitch_align = 0.0*3.141562/180.0;
+ roll_align = 0.0*3.141562/180.0;
+ float ave_pitch = 0.0;
+ float ave_roll = 0.0;
+ for (int i = 0 ; i<2200; i++){
+ float tstart = t.read();
+ //姿勢角を更新
+ getIMUval();
+ updateBetweenMeasures();
+ updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
+ updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
+ computeAngles();
+ if(i>199){
+ ave_pitch += pitch;
+ ave_roll += roll;
+ }
+ wait(0.001);
+ float tend = t.read();
+ att_dt = (tend-tstart);
+ }
+ pc.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0*180.0f/pi,ave_roll/2000.0*180.0f/pi);
pc.printf("Writing to EEPROM...\r\n");
U transfer_data;
@@ -699,6 +724,16 @@
}
}
+ // ave_pitch,ave_rollを送る
+ int ave_pr[2] = {ave_pitch*100, ave_roll*100};
+ for (int i = 8; i < 10; i++) {
+ if (!isnan(ave_pr[i - 8]))
+ transfer_data.i[i] = ave_pr[i - 8];
+ else {
+ pc.printf("alignment data is NOT transferred\n");
+ transfer_data.i[i] = 0;
+ }
+ }
for (int i = 0; i < N_EEPROM; i++) {
pc.printf("transfer_data[%d]: %\d\r\n", i, transfer_data.i[i]);
}
@@ -712,22 +747,6 @@
pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]);
}
- float pitch_align = 0.0*3.141562/180.0
- float roll_align = 0.0*3.141562/180.0
-
- for (int i =1 ; i<1000; i++){
- float tstart = t.read();
- //姿勢角を更新
- getIMUval();
- updateBetweenMeasures();
- updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
- updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
- computeAngles();
-
- float tend = t.read();
- att_dt = (tend-tstart);
- }
-
while(1) {wait(1000);}
}
}
\ No newline at end of file