HAPSRG
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:
- 56:888379912f81
- Parent:
- 54:ca88777b295a
- Child:
- 58:a7947322db87
--- a/main.cpp Mon May 31 07:27:42 2021 +0000
+++ b/main.cpp Mon May 31 18:59:36 2021 +0000
@@ -1,797 +1,16 @@
-//#include "mbed.h"
-//#include "PIDcontroller.h"
-//#include "SBUS.hpp"
-//#include "LoopTicker.hpp"
-//#include "MPU6050.h"
-//#include "MAG3110.h"
-//#include "I2Cdev.h"
-//#include "FastPWM.h"
-//#include "Matrix.h"
-//#include "MatrixMath.h"
-//#include "math.h"
-//#include "UsaPack.hpp"
-//
-//#define MPU6050_PWR_MGMT_1 0x6B
-//#define MPU_ADDRESS 0x68
-//#define pi 3.141562
-//#define ACCEL_FSR MPU6050_ACCEL_FS_8
-//#define ACCEL_SSF 4096.0
-//#define GYRO_FSR MPU6050_GYRO_FS_250
-//#define GYRO_SSF 131.0
-//#define MPU6050_LPF MPU6050_DLPF_BW_256
-//#define gyro_th 20.0
-//#define PID_dt 0.015
-//#define servoPwmMax 1800.0
-//#define servoPwmMin 1200.0
-//#define motorPwmMax 2000.0
-//#define motorPwmMin 1100.0
-//
-//#define N_EEPROM 11
-//
-//MPU6050 accelgyro;
-//MAG3110 mag(PB_9,PB_8);
-//SBUS sbus(PD_5, PD_6);
-//Serial pc(USBTX, USBRX, 115200);
-//DigitalIn userButton(USER_BUTTON);
-//DigitalIn locdef1(PG_2);
-//DigitalIn locdef2(PG_3);
-//FastPWM servo(PE_9);
-//PID pitchPID(5.0, 0,0,PID_dt); //rad
-//PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
-//Timer t;
-//
-//Matrix qhat(4,1);
-//Matrix qhat_gyro(4,1);
-//Matrix Phat(4,4);
-//Matrix Qgyro(3,3);
-//Matrix Racc(3,3);
-//Matrix Rmag(3,3);
-//Matrix D(3,3);
-//
-//int loop_count = 0;
-//int obs_count = 0;
-//float att_dt = 0.01;
-//float rc[16];
-//float pitch = 0.0;
-//float roll = 0.0;
-//float yaw = 0.0;
-//
-//float pitch_g = 0.0;
-//float roll_g = 0.0;
-//float yaw_g = 0.0;
-//
-//int16_t ax, ay, az;
-//int16_t gx, gy, gz;
-//MotionSensorDataUnits mdata;
-//float magval[3] = {1.0,0.0,0.0};
-//float acc_x,acc_y,acc_z;
-//float dynacc_x,dynacc_y,dynacc_z;
-//float gyro_x,gyro_y,gyro_z;
-//float mag_x,mag_y,mag_z;
-//
-//float LPacc_x=0.0;
-//float LPacc_y=0.0;
-//float LPacc_z=0.0;
-//float LPmag_x = 0.0;
-//float LPmag_y = 0.0;
-//float LPmag_z = 0.0;;
-//
-//int out1, out2;
-//float scaledServoOut[1]= {0};
-//float servoOut[1] = {1500.0};
-//
-//float accnorm;
-//float magnorm;
-//float LPaccnorm;
-//float LPmagnorm;
-//float accrefnorm;
-//float magrefnorm;
-//
-//
-//float val_thmg = 0.0;
-//float th_mg = 0.0;
-//float dynaccnorm = 0.0;
-//float accnormerr = 0.0;
-//float accref[3] = {0, 0, -0.98};
-//float magref[3] = {0.65, 0, 0.75};
-//
-//int calibrationFlag = 0;
-//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;
-//// eepromのread writeのためのunionを定義
-//int address = 0xAE; // EEPROMの3つの入力が全て+より
-//int pointeraddress = 0;
-//
-//I2C i2c(PB_9,PB_8); // sda, scl
-//union U{
-// int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
-// 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]);
-//// 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("%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書き込み・読み込みに必要な関数
-//void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
-//{
-// char i2cBuffer[size + 2];
-// i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
-// i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
-//
-// for (int i = 0; i < size; i++) {
-// i2cBuffer[i + 2] = data[i];
-// }
-//
-// int result = i2c.write(address, i2cBuffer, size + 2, false);
-// //sleep_ms(500);
-//}
-//
-//// this function has no read limit
-//void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
-//{
-// char i2cBuffer[2];
-// i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
-// i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
-//
-// // Reset eeprom pointer address
-// int result = i2c.write(address, i2cBuffer, 2, false);
-//
-// //sleep_ms(500);
-//
-// // Read eeprom
-// i2c.read(address, data, size);
-// //sleep_ms(500);
-//}
-//
-//float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
-//{
-// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-//}
-//
-//void calcMagRef(float mx, float my, float mz){
-//
-// Matrix magvec(3,1);
-// magvec << mx << my << mz;
-// Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
-// magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
-// magref[1] = 0.0;
-// magref[2] = magnedvec(3,1);
-//}
-//
-//
-//void getIMUval(){
-// // gx gy gz ax ay az
-// accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
-// ax = ax - agoffset[0];
-// ay = ay - agoffset[1];
-// az = -az - agoffset[2];
-// gx = gx - agoffset[3];
-// gy = gy - agoffset[4];
-// gz = -gz - agoffset[5];
-// // 加速度値を分解能で割って加速度(G)に変換する
-// acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
-// acc_y = float(ay) / ACCEL_SSF;
-// acc_z = float(az) / ACCEL_SSF;
-// // 角速度値を分解能で割って角速度(rad per sec)に変換する
-// gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
-// gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
-// gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
-// mag.getAxis(mdata); // flush the magnetmeter
-// magval[0] = (mdata.x - magbias[0]);
-// magval[1] = (mdata.y - magbias[1]);
-// magval[2] = (mdata.z - magbias[2]);
-// mag_x = -magval[0]/magbias[3];
-// mag_y = -magval[1]/magbias[3];
-// mag_z = -magval[2]/magbias[3];
-//
-// float lpc_acc = 0.15;
-// LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
-// LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
-// LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;
-//
-// float lpc_mag = 0.15;
-// LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
-// LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
-// LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
-//
-// accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
-// magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
-// LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
-// LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
-// accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
-// magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
-// calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
-//
-//
-//}
-//
-//void updateBetweenMeasures(){
-// float q0 = qhat.getNumber( 1, 1 );
-// float q1 = qhat.getNumber( 2, 1 );
-// float q2 = qhat.getNumber( 3, 1 );
-// float q3 = qhat.getNumber( 4, 1 );
-//
-// Matrix B(4,3);
-// B << q1 << q2 << q3
-// <<-q0 << q3 <<-q2
-// <<-q3 <<-q0 << q1
-// << q2 <<-q1 <<-q0;
-// B *= 0.5f;
-//
-// Matrix phi(4,4);
-// phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
-// << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
-// << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
-// << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
-//
-// qhat = phi*qhat;
-// float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
-// qhat *= (1.0f/ qnorm);
-//
-// qhat_gyro = phi*qhat_gyro;
-// qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
-// qhat_gyro *= (1.0f/ qnorm);
-//
-// Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
-//
-// q0 = qhat.getNumber( 1, 1 );
-// q1 = qhat.getNumber( 2, 1 );
-// q2 = qhat.getNumber( 3, 1 );
-// q3 = qhat.getNumber( 4, 1 );
-//
-// D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
-// D.add(1,2,2*(q1*q2 + q0*q3));
-// D.add(1,3,2*(q1*q3 - q0*q2));
-// D.add(2,1,2*(q1*q2 - q0*q3));
-// D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
-// D.add(2,3,2*(q2*q3 + q0*q1));
-// D.add(3,1,2*(q1*q3 + q0*q2));
-// D.add(3,2,2*(q2*q3 - q0*q1));
-// D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
-//
-//}
-//
-//void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
-//
-// Matrix u(3,1);
-// Matrix v(3,1);
-//
-// u << u1 << u2 <<u3;
-// v << v1 << v2 <<v3;
-//
-// float q0 = qhat.getNumber( 1, 1 );
-// float q1 = qhat.getNumber( 2, 1 );
-// float q2 = qhat.getNumber( 3, 1 );
-// float q3 = qhat.getNumber( 4, 1 );
-//
-// Matrix A1(3,3);
-// A1 << q0 << q3 << -q2
-// <<-q3 << q0 << q1
-// <<q2 <<-q1 <<q0;
-// A1 *= 2.0f;
-//
-// Matrix A2(3,3);
-// A2 << q1 << q2 << q3
-// << q2 <<-q1 << q0
-// << q3 <<-q0 <<-q1;
-// A2 *= 2.0f;
-//
-// Matrix A3(3,3);
-// A3 <<-q2 << q1 <<-q0
-// << q1 << q2 << q3
-// << q0 << q3 <<-q2;
-// A3 *= 2.0f;
-//
-// Matrix A4(3,3);
-// A4 <<-q3 << q0 << q1
-// <<-q0 <<-q3 << q2
-// << q1 << q2 << q3;
-// A4 *= 2.0f;
-//
-// Matrix H(3,4);
-//
-// Matrix ab1(A1*u);
-// Matrix ab2(A2*u);
-// Matrix ab3(A3*u);
-// Matrix ab4(A4*u);
-//
-// H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
-// << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
-// << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
-//
-//
-// Matrix K(4,3);
-// K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-//
-// Matrix dq(4,1);
-// dq = K*(v-D*u);
-// qhat = qhat+dq;
-//
-// float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
-// qhat *= (1.0f/ qnorm);
-//
-// Matrix eye4(4,4);
-// eye4 << 1 << 0 << 0 << 0
-// << 0 << 1 << 0 << 0
-// << 0 << 0 << 1 << 0
-// << 0 << 0 << 0 << 1;
-// Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
-//}
-//
-//void computeAngles()
-//{
-// float q0 = qhat.getNumber( 1, 1 );
-// float q1 = qhat.getNumber( 2, 1 );
-// float q2 = qhat.getNumber( 3, 1 );
-// float q3 = qhat.getNumber( 4, 1 );
-// roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
-// pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
-// yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
-//
-// q0 = qhat_gyro.getNumber( 1, 1 );
-// q1 = qhat_gyro.getNumber( 2, 1 );
-// q2 = qhat_gyro.getNumber( 3, 1 );
-// q3 = qhat_gyro.getNumber( 4, 1 );
-// roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
-// pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
-// yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
-//
-//}
-//
-//void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){
-// Matrix W1(3,1);
-// W1 << fb1 << fb2<< fb3;
-// Matrix W2(3,1);
-// W2 << mb1 << mb2<< mb3;
-//
-// Matrix V1(3,1);
-// V1 << fn1 << fn2<< fn3;
-// Matrix V2(3,1);
-// V2 << mn1 << mn2<< mn3;
-//
-//
-// Matrix Ou2(3,1);
-// Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 );
-// Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
-// Matrix Ou3(3,1);
-// Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 );
-// Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
-// Matrix R2(3,1);
-// R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 );
-// R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
-// Matrix R3(3,1);
-// R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 );
-// R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
-//
-// Matrix Mou(3,3);
-// Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
-// << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
-// << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
-// Matrix Mr(3,3);
-// Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
-// << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
-// << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
-//
-// Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
-//
-// float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
-//
-// qhat.add(1,1,0.5*sqtrp1);
-// qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
-// qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
-// qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
-//
-// float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
-// qhat *= (1.0f/ qnorm);
-//
-// qhat_gyro = qhat;
-//
-// float q0 = qhat.getNumber( 1, 1 );
-// float q1 = qhat.getNumber( 2, 1 );
-// float q2 = qhat.getNumber( 3, 1 );
-// float q3 = qhat.getNumber( 4, 1 );
-//
-// D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
-// D.add(1,2,2*(q1*q2 + q0*q3));
-// D.add(1,3,2*(q1*q3 - q0*q2));
-// D.add(2,1,2*(q1*q2 - q0*q3));
-// D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
-// D.add(2,3,2*(q2*q3 + q0*q1));
-// D.add(3,1,2*(q1*q3 + q0*q2));
-// D.add(3,2,2*(q2*q3 - q0*q1));
-// D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
-//
-//}
-//
-//void calcDynAcc(){
-// dynacc_x = LPacc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]);
-// dynacc_y = LPacc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]);
-// dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
-//}
-//
-//// 割り込まれた時点での出力(computeの結果）を返す関数
-//void calcServoOut(){
-// if(sbus.failSafe == false) {
-// // 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
-// }
-// }
-// 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];
-//
-// scaledServoOut[0]=de;
-//
-// float LP_servo = 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;
-// };
-// }
-//
-// servo.pulsewidth_us(servoOut[0]);
-//
-// //観測アップデート
-// calcDynAcc();
-// th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
-// dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
-// accnormerr = abs(LPaccnorm-accrefnorm);
-// //静止時100個の平均 0.01877522 0.00514146 0.00477393
-//
-// //int ang_th = th_mg < 0.01877522;
-// //int dyn_th = dynaccnorm < 0.00514146;
-// //int norm_th = accnormerr< 0.00477393;
-// int ang_th = th_mg < 0.01877522/5.0;
-// int dyn_th = dynaccnorm < 0.00514146/5.0;
-// int norm_th = accnormerr< 0.00477393/5.0;
-// if(dyn_th+ang_th+norm_th>0){
-// //if(dyn_th+ang_th+norm_th>-1){
-// obs_count += 1;
-// 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);
-// }
-//
-// if(loop_count >= 10){
-// writeSdcard();
-// loop_count = 1;
-// obs_count = 0;
-// }else{
-// loop_count +=1;
-// }
-//
-//
-//}
-//
-//int main()
-//{
-// pitchPID.setSetPoint(0.0);
-// pitchratePID.setSetPoint(0.0);
-// pitchPID.setBias(0.0);
-// pitchratePID.setBias(0.0);
-// pitchPID.setOutputLimits(-1.0,1.0);
-// pitchratePID.setOutputLimits(-1.0,1.0);
-// pitchPID.setInputLimits(-pi,pi);
-// pitchratePID.setInputLimits(-pi,pi);
-//
-// servo.period_us(15000.0);
-// servo.pulsewidth_us(1500.0);
-//
-//// pc.baud(57600);
-// accelgyro.initialize();
-// //加速度計のフルスケールレンジを設定
-// accelgyro.setFullScaleAccelRange(ACCEL_FSR);
-// //角速度計のフルスケールレンジを設定
-// accelgyro.setFullScaleGyroRange(GYRO_FSR);
-// //MPU6050のLPFを設定
-// accelgyro.setDLPFMode(MPU6050_LPF);
-// //地磁気
-// mag.enable();
-//
-// qhat << 1 << 0 << 0 << 0;
-//
-// D.add(1,1,1.0);
-// D.add(2,2,1.0);
-// D.add(3,3,1.0);
-//
-// 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*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);
-// Rmag.add(3,3,1.0);
-//
-// calibrationFlag = userButton.read();
-// if(calibrationFlag == 0){
-//
-// pc.printf("reading calibration value\r\n");
-// //キャリブレーション値を取得
-// U read_calib;
-// readEEPROM(address, pointeraddress, read_calib.c, N_EEPROM*4);
-// wait(3);
-// pos_tail = read_calib.i[0];
-// agoffset[3] = float(read_calib.i[5]);
-// agoffset[4] = float(read_calib.i[6]);
-// agoffset[5] = float(read_calib.i[7]);
-// magbias[0] = float(read_calib.i[1])/1000.0;
-// 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;
-// tail_address = (int)read_calib.i[10];
-//
-// switch(pos_tail){
-// case 1:
-// pc.printf("This MBED is Located at Left \r\n");
-// break;
-// case 2:
-// pc.printf("This MBED is Located at Center \r\n");
-// break;
-// case 3:
-// pc.printf("This MBED is Located at Right \r\n");
-// break;
-// default: // error situation
-// 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);
-// float sumLPaccnorm = 0;
-// for(int i = 0; i<1000 ;i++){
-// getIMUval();
-// val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
-// sumLPaccnorm += LPaccnorm;
-// }
-// accref[2]=-sumLPaccnorm/1000;
-// val_thmg /= 1000;
-// LoopTicker PIDtick;
-// PIDtick.attach(calcServoOut,PID_dt);
-//
-// t.start();
-//
-// while(1) {
-// float tstart = t.read();
-// //姿勢角を更新
-// getIMUval();
-// updateBetweenMeasures();
-// computeAngles();
-// PIDtick.loop();
-//
-// float tend = t.read();
-// att_dt = (tend-tstart);
-// }
-// }else{
-// pc.printf("\r\nEnter to Calibration Mode\r\n");
-// wait(5);
-// pc.printf("Acc and Gyro Calibration Start\r\n");
-//
-// int gxs = 0;
-// int gys = 0;
-// int gzs = 0;
-// int iter_n = 10000;
-// for(int i = 0;i<iter_n ;i++){
-// accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
-// gxs += gx;
-// gys += gy;
-// gzs -= gz;
-// //wait(0.01);
-// }
-// gxs = gxs /iter_n;
-// gys = gys /iter_n;
-// gzs = gzs /iter_n;
-// agoffset[3] = gxs;
-// agoffset[4] = gys;
-// agoffset[5] = gzs;
-// pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
-//
-// pc.printf("Mag Calibration Start\r\n");
-// float f = 0;
-// while(1){
-// mag.getAxis(mdata); // flush the magnetmeter
-// magval[0] = (mdata.x - magbias[0]);
-// magval[1] = (mdata.y - magbias[1]);
-// magval[2] = (mdata.z - magbias[2]);
-// float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
-// float lr = 0.00001f;
-// f = mag_r - magbias[3]*magbias[3];
-// magbias[0] = magbias[0] + 4 * lr * f * magval[0];
-// magbias[1] = magbias[1] + 4 * lr * f * magval[1];
-// magbias[2] = magbias[2] + 4 * lr * f * magval[2];
-// magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
-//
-// if(userButton.read() == 1){
-// break;
-// }
-// wait(0.001);
-// }
-// pc.printf("Magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
-//
-// pc.printf("Determine Position of MBED\r\n");
-// wait(1);
-// pc.printf("Press the user button\r\n");
-//
-// while(userButton.read() == 0){wait(0.01);};
-// while(1){
-// pc.printf("Left\r\n");
-// 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");
-// break;
-// case 2:
-// pc.printf("This MBED is Located at Center \r\n");
-// break;
-// case 3:
-// pc.printf("This MBED is Located at Right \r\n");
-// break;
-// default: // error situation
-// pc.printf("error\r\n");
-// 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;
-// transfer_data.i[0] = pos_tail;
-// for (int i = 1; i < 5; i++) {
-// if (!isnan(magbias[i - 1]))
-// transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。
-// else {
-// pc.printf("Mag bias is NOT transferred\n");
-// transfer_data.i[i] = 100;
-// }
-// }
-// // gxs,gys,gzsを送る
-// int gxyzs[3] = {gxs, gys, gzs};
-// for (int i = 5; i < 8; i++) {
-// if (!isnan(gxyzs[i - 5]))
-// transfer_data.i[i] = gxyzs[i - 5];
-// else {
-// pc.printf("gxyzs is NOT transferred\n");
-// transfer_data.i[i] = 0;
-// }
-// }
-//
-// // 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;
-// }
-// }
-// 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]);
-// }
-// writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4);
-// wait(3);
-//
-// U read_test;
-// readEEPROM(address, pointeraddress, read_test.c, N_EEPROM*4);
-// wait(3);
-// for (int i = 0 ; i < N_EEPROM; i ++){
-// pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]);
-// }
-//
-// while(1) {wait(1000);}
-// }
-//}
\ No newline at end of file
+#include "global.hpp"
+
+int main()
+{
+ setup();
+
+ calibrationFlag = userButton.read();
+ if(calibrationFlag == 0)
+ {
+ run();
+ }
+ else
+ {
+ calibrate();
+ }
+}
\ No newline at end of file