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:
- 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