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