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:
- 54:ca88777b295a
- Parent:
- 53:3a6dfb55e087
- Child:
- 56:888379912f81
--- a/main.cpp Mon May 24 05:45:56 2021 +0000
+++ b/main.cpp Mon May 31 07:15:52 2021 +0000
@@ -1,797 +1,797 @@
-#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 "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