HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

main.cpp@54:ca88777b295a, 2021-05-31 (annotated)

Committer:
cocorlow
Date:
Mon May 31 07:15:52 2021 +0000
Revision:
54:ca88777b295a
Parent:
53:3a6dfb55e087
Child:
56:888379912f81
ekf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 54:ca88777b295a 1 //#include "mbed.h"
cocorlow 54:ca88777b295a 2 //#include "PIDcontroller.h"
cocorlow 54:ca88777b295a 3 //#include "SBUS.hpp"
cocorlow 54:ca88777b295a 4 //#include "LoopTicker.hpp"
cocorlow 54:ca88777b295a 5 //#include "MPU6050.h"
cocorlow 54:ca88777b295a 6 //#include "MAG3110.h"
cocorlow 54:ca88777b295a 7 //#include "I2Cdev.h"
cocorlow 54:ca88777b295a 8 //#include "FastPWM.h"
cocorlow 54:ca88777b295a 9 //#include "Matrix.h"
cocorlow 54:ca88777b295a 10 //#include "MatrixMath.h"
cocorlow 54:ca88777b295a 11 //#include "math.h"
cocorlow 54:ca88777b295a 12 //#include "UsaPack.hpp"
cocorlow 54:ca88777b295a 13 //
cocorlow 54:ca88777b295a 14 //#define MPU6050_PWR_MGMT_1 0x6B
cocorlow 54:ca88777b295a 15 //#define MPU_ADDRESS 0x68
cocorlow 54:ca88777b295a 16 //#define pi 3.141562
cocorlow 54:ca88777b295a 17 //#define ACCEL_FSR MPU6050_ACCEL_FS_8
cocorlow 54:ca88777b295a 18 //#define ACCEL_SSF 4096.0
cocorlow 54:ca88777b295a 19 //#define GYRO_FSR MPU6050_GYRO_FS_250
cocorlow 54:ca88777b295a 20 //#define GYRO_SSF 131.0
cocorlow 54:ca88777b295a 21 //#define MPU6050_LPF MPU6050_DLPF_BW_256
cocorlow 54:ca88777b295a 22 //#define gyro_th 20.0
cocorlow 54:ca88777b295a 23 //#define PID_dt 0.015
cocorlow 54:ca88777b295a 24 //#define servoPwmMax 1800.0
cocorlow 54:ca88777b295a 25 //#define servoPwmMin 1200.0
cocorlow 54:ca88777b295a 26 //#define motorPwmMax 2000.0
cocorlow 54:ca88777b295a 27 //#define motorPwmMin 1100.0
cocorlow 54:ca88777b295a 28 //
cocorlow 54:ca88777b295a 29 //#define N_EEPROM 11
cocorlow 54:ca88777b295a 30 //
cocorlow 54:ca88777b295a 31 //MPU6050 accelgyro;
cocorlow 54:ca88777b295a 32 //MAG3110 mag(PB_9,PB_8);
cocorlow 54:ca88777b295a 33 //SBUS sbus(PD_5, PD_6);
cocorlow 54:ca88777b295a 34 //Serial pc(USBTX, USBRX, 115200);
cocorlow 54:ca88777b295a 35 //DigitalIn userButton(USER_BUTTON);
cocorlow 54:ca88777b295a 36 //DigitalIn locdef1(PG_2);
cocorlow 54:ca88777b295a 37 //DigitalIn locdef2(PG_3);
cocorlow 54:ca88777b295a 38 //FastPWM servo(PE_9);
cocorlow 54:ca88777b295a 39 //PID pitchPID(5.0, 0,0,PID_dt); //rad
cocorlow 54:ca88777b295a 40 //PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
cocorlow 54:ca88777b295a 41 //Timer t;
cocorlow 54:ca88777b295a 42 //
cocorlow 54:ca88777b295a 43 //Matrix qhat(4,1);
cocorlow 54:ca88777b295a 44 //Matrix qhat_gyro(4,1);
cocorlow 54:ca88777b295a 45 //Matrix Phat(4,4);
cocorlow 54:ca88777b295a 46 //Matrix Qgyro(3,3);
cocorlow 54:ca88777b295a 47 //Matrix Racc(3,3);
cocorlow 54:ca88777b295a 48 //Matrix Rmag(3,3);
cocorlow 54:ca88777b295a 49 //Matrix D(3,3);
cocorlow 54:ca88777b295a 50 //
cocorlow 54:ca88777b295a 51 //int loop_count = 0;
cocorlow 54:ca88777b295a 52 //int obs_count = 0;
cocorlow 54:ca88777b295a 53 //float att_dt = 0.01;
cocorlow 54:ca88777b295a 54 //float rc[16];
cocorlow 54:ca88777b295a 55 //float pitch = 0.0;
cocorlow 54:ca88777b295a 56 //float roll = 0.0;
cocorlow 54:ca88777b295a 57 //float yaw = 0.0;
cocorlow 54:ca88777b295a 58 //
cocorlow 54:ca88777b295a 59 //float pitch_g = 0.0;
cocorlow 54:ca88777b295a 60 //float roll_g = 0.0;
cocorlow 54:ca88777b295a 61 //float yaw_g = 0.0;
cocorlow 54:ca88777b295a 62 //
cocorlow 54:ca88777b295a 63 //int16_t ax, ay, az;
cocorlow 54:ca88777b295a 64 //int16_t gx, gy, gz;
cocorlow 54:ca88777b295a 65 //MotionSensorDataUnits mdata;
cocorlow 54:ca88777b295a 66 //float magval[3] = {1.0,0.0,0.0};
cocorlow 54:ca88777b295a 67 //float acc_x,acc_y,acc_z;
cocorlow 54:ca88777b295a 68 //float dynacc_x,dynacc_y,dynacc_z;
cocorlow 54:ca88777b295a 69 //float gyro_x,gyro_y,gyro_z;
cocorlow 54:ca88777b295a 70 //float mag_x,mag_y,mag_z;
cocorlow 54:ca88777b295a 71 //
cocorlow 54:ca88777b295a 72 //float LPacc_x=0.0;
cocorlow 54:ca88777b295a 73 //float LPacc_y=0.0;
cocorlow 54:ca88777b295a 74 //float LPacc_z=0.0;
cocorlow 54:ca88777b295a 75 //float LPmag_x = 0.0;
cocorlow 54:ca88777b295a 76 //float LPmag_y = 0.0;
cocorlow 54:ca88777b295a 77 //float LPmag_z = 0.0;;
cocorlow 54:ca88777b295a 78 //
cocorlow 54:ca88777b295a 79 //int out1, out2;
cocorlow 54:ca88777b295a 80 //float scaledServoOut[1]= {0};
cocorlow 54:ca88777b295a 81 //float servoOut[1] = {1500.0};
cocorlow 54:ca88777b295a 82 //
cocorlow 54:ca88777b295a 83 //float accnorm;
cocorlow 54:ca88777b295a 84 //float magnorm;
cocorlow 54:ca88777b295a 85 //float LPaccnorm;
cocorlow 54:ca88777b295a 86 //float LPmagnorm;
cocorlow 54:ca88777b295a 87 //float accrefnorm;
cocorlow 54:ca88777b295a 88 //float magrefnorm;
cocorlow 54:ca88777b295a 89 //
cocorlow 54:ca88777b295a 90 //
cocorlow 54:ca88777b295a 91 //float val_thmg = 0.0;
cocorlow 54:ca88777b295a 92 //float th_mg = 0.0;
cocorlow 54:ca88777b295a 93 //float dynaccnorm = 0.0;
cocorlow 54:ca88777b295a 94 //float accnormerr = 0.0;
cocorlow 54:ca88777b295a 95 //float accref[3] = {0, 0, -0.98};
cocorlow 54:ca88777b295a 96 //float magref[3] = {0.65, 0, 0.75};
cocorlow 54:ca88777b295a 97 //
cocorlow 54:ca88777b295a 98 //int calibrationFlag = 0;
cocorlow 54:ca88777b295a 99 //int pos_tail = 1;//1:left 2:center 3:right
cocorlow 54:ca88777b295a 100 //int agoffset[6] = {0, 0, 0, 386, -450, 48};
cocorlow 54:ca88777b295a 101 //float magbias[4] = {-215.833374, 207.740616, -167.444565, 36.688690};
cocorlow 54:ca88777b295a 102 //float pitch_align = 0.0*3.141562/180.0;
cocorlow 54:ca88777b295a 103 //float roll_align = 0.0*3.141562/180.0;
cocorlow 54:ca88777b295a 104 //// eepromのread writeのためのunionを定義
cocorlow 54:ca88777b295a 105 //int address = 0xAE; // EEPROMの3つの入力が全て+より
cocorlow 54:ca88777b295a 106 //int pointeraddress = 0;
cocorlow 54:ca88777b295a 107 //
cocorlow 54:ca88777b295a 108 //I2C i2c(PB_9,PB_8); // sda, scl
cocorlow 54:ca88777b295a 109 //union U{
cocorlow 54:ca88777b295a 110 // int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
cocorlow 54:ca88777b295a 111 // char c[N_EEPROM*4]; // floatはcharの4倍
cocorlow 54:ca88777b295a 112 //};
cocorlow 54:ca88777b295a 113 //
cocorlow 54:ca88777b295a 114 //// UsaPack
cocorlow 54:ca88777b295a 115 //UsaPack tail(PG_14, PG_9, 115200);
cocorlow 54:ca88777b295a 116 //int tail_address = 0;
cocorlow 54:ca88777b295a 117 //struct valuePack
cocorlow 54:ca88777b295a 118 //{
cocorlow 54:ca88777b295a 119 // float dt;
cocorlow 54:ca88777b295a 120 // int count;
cocorlow 54:ca88777b295a 121 // float acc[3];
cocorlow 54:ca88777b295a 122 // float gyro[3];
cocorlow 54:ca88777b295a 123 // float mag[3];
cocorlow 54:ca88777b295a 124 // float rot[3];
cocorlow 54:ca88777b295a 125 // float rot_g[3];
cocorlow 54:ca88777b295a 126 //};
cocorlow 54:ca88777b295a 127 //valuePack vp;
cocorlow 54:ca88777b295a 128 //
cocorlow 54:ca88777b295a 129 //
cocorlow 54:ca88777b295a 130 //void writeSdcard()
cocorlow 54:ca88777b295a 131 //{
cocorlow 54:ca88777b295a 132 // //magcal.getExtremes(&magmin[0],&magmax[0]);
cocorlow 54:ca88777b295a 133 // //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
cocorlow 54:ca88777b295a 134 // //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]);
cocorlow 54:ca88777b295a 135 //// 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);
cocorlow 54:ca88777b295a 136 // //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);
cocorlow 54:ca88777b295a 137 // //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);
cocorlow 54:ca88777b295a 138 // //pc.printf("%d \r\n",userButton.read());
cocorlow 54:ca88777b295a 139 // //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);
cocorlow 54:ca88777b295a 140 // //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
cocorlow 54:ca88777b295a 141 // //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);
cocorlow 54:ca88777b295a 142 // //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
cocorlow 54:ca88777b295a 143 // //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);
cocorlow 54:ca88777b295a 144 //
cocorlow 54:ca88777b295a 145 // vp.dt = 1.0f/att_dt;
cocorlow 54:ca88777b295a 146 // vp.count = obs_count;
cocorlow 54:ca88777b295a 147 // vp.acc[0] = acc_x;
cocorlow 54:ca88777b295a 148 // vp.acc[1] = acc_y;
cocorlow 54:ca88777b295a 149 // vp.acc[2] = acc_z;
cocorlow 54:ca88777b295a 150 // vp.gyro[0] = gyro_x;
cocorlow 54:ca88777b295a 151 // vp.gyro[1] = gyro_y;
cocorlow 54:ca88777b295a 152 // vp.gyro[2] = gyro_z;
cocorlow 54:ca88777b295a 153 // vp.mag[0] = mag_x;
cocorlow 54:ca88777b295a 154 // vp.mag[1] = mag_y;
cocorlow 54:ca88777b295a 155 // vp.mag[2] = mag_z;
cocorlow 54:ca88777b295a 156 // vp.rot[0] = roll*180.0f/pi;
cocorlow 54:ca88777b295a 157 // vp.rot[1] = pitch*180.0f/pi;
cocorlow 54:ca88777b295a 158 // vp.rot[2] = yaw*180.0f/pi;
cocorlow 54:ca88777b295a 159 // vp.rot_g[0] = roll_g*180.0f/pi;
cocorlow 54:ca88777b295a 160 // vp.rot_g[1] = pitch_g*180.0f/pi;
cocorlow 54:ca88777b295a 161 // vp.rot_g[2] = yaw_g*180.0f/pi;
cocorlow 54:ca88777b295a 162 // tail.Send(tail_address, &vp);
cocorlow 54:ca88777b295a 163 //// 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);
cocorlow 54:ca88777b295a 164 //
cocorlow 54:ca88777b295a 165 //}
cocorlow 54:ca88777b295a 166 //
cocorlow 54:ca88777b295a 167 //// eeprom書き込み・読み込みに必要な関数
cocorlow 54:ca88777b295a 168 //void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
cocorlow 54:ca88777b295a 169 //{
cocorlow 54:ca88777b295a 170 // char i2cBuffer[size + 2];
cocorlow 54:ca88777b295a 171 // i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
cocorlow 54:ca88777b295a 172 // i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
cocorlow 54:ca88777b295a 173 //
cocorlow 54:ca88777b295a 174 // for (int i = 0; i < size; i++) {
cocorlow 54:ca88777b295a 175 // i2cBuffer[i + 2] = data[i];
cocorlow 54:ca88777b295a 176 // }
cocorlow 54:ca88777b295a 177 //
cocorlow 54:ca88777b295a 178 // int result = i2c.write(address, i2cBuffer, size + 2, false);
cocorlow 54:ca88777b295a 179 // //sleep_ms(500);
cocorlow 54:ca88777b295a 180 //}
cocorlow 54:ca88777b295a 181 //
cocorlow 54:ca88777b295a 182 //// this function has no read limit
cocorlow 54:ca88777b295a 183 //void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
cocorlow 54:ca88777b295a 184 //{
cocorlow 54:ca88777b295a 185 // char i2cBuffer[2];
cocorlow 54:ca88777b295a 186 // i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
cocorlow 54:ca88777b295a 187 // i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
cocorlow 54:ca88777b295a 188 //
cocorlow 54:ca88777b295a 189 // // Reset eeprom pointer address
cocorlow 54:ca88777b295a 190 // int result = i2c.write(address, i2cBuffer, 2, false);
cocorlow 54:ca88777b295a 191 //
cocorlow 54:ca88777b295a 192 // //sleep_ms(500);
cocorlow 54:ca88777b295a 193 //
cocorlow 54:ca88777b295a 194 // // Read eeprom
cocorlow 54:ca88777b295a 195 // i2c.read(address, data, size);
cocorlow 54:ca88777b295a 196 // //sleep_ms(500);
cocorlow 54:ca88777b295a 197 //}
cocorlow 54:ca88777b295a 198 //
cocorlow 54:ca88777b295a 199 //float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
cocorlow 54:ca88777b295a 200 //{
cocorlow 54:ca88777b295a 201 // return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 54:ca88777b295a 202 //}
cocorlow 54:ca88777b295a 203 //
cocorlow 54:ca88777b295a 204 //void calcMagRef(float mx, float my, float mz){
cocorlow 54:ca88777b295a 205 //
cocorlow 54:ca88777b295a 206 // Matrix magvec(3,1);
cocorlow 54:ca88777b295a 207 // magvec << mx << my << mz;
cocorlow 54:ca88777b295a 208 // Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
cocorlow 54:ca88777b295a 209 // magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
cocorlow 54:ca88777b295a 210 // magref[1] = 0.0;
cocorlow 54:ca88777b295a 211 // magref[2] = magnedvec(3,1);
cocorlow 54:ca88777b295a 212 //}
cocorlow 54:ca88777b295a 213 //
cocorlow 54:ca88777b295a 214 //
cocorlow 54:ca88777b295a 215 //void getIMUval(){
cocorlow 54:ca88777b295a 216 // // gx gy gz ax ay az
cocorlow 54:ca88777b295a 217 // accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
cocorlow 54:ca88777b295a 218 // ax = ax - agoffset[0];
cocorlow 54:ca88777b295a 219 // ay = ay - agoffset[1];
cocorlow 54:ca88777b295a 220 // az = -az - agoffset[2];
cocorlow 54:ca88777b295a 221 // gx = gx - agoffset[3];
cocorlow 54:ca88777b295a 222 // gy = gy - agoffset[4];
cocorlow 54:ca88777b295a 223 // gz = -gz - agoffset[5];
cocorlow 54:ca88777b295a 224 // // 加速度値を分解能で割って加速度(G)に変換する
cocorlow 54:ca88777b295a 225 // acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
cocorlow 54:ca88777b295a 226 // acc_y = float(ay) / ACCEL_SSF;
cocorlow 54:ca88777b295a 227 // acc_z = float(az) / ACCEL_SSF;
cocorlow 54:ca88777b295a 228 // // 角速度値を分解能で割って角速度(rad per sec)に変換する
cocorlow 54:ca88777b295a 229 // gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
cocorlow 54:ca88777b295a 230 // gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
cocorlow 54:ca88777b295a 231 // gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
cocorlow 54:ca88777b295a 232 // mag.getAxis(mdata); // flush the magnetmeter
cocorlow 54:ca88777b295a 233 // magval[0] = (mdata.x - magbias[0]);
cocorlow 54:ca88777b295a 234 // magval[1] = (mdata.y - magbias[1]);
cocorlow 54:ca88777b295a 235 // magval[2] = (mdata.z - magbias[2]);
cocorlow 54:ca88777b295a 236 // mag_x = -magval[0]/magbias[3];
cocorlow 54:ca88777b295a 237 // mag_y = -magval[1]/magbias[3];
cocorlow 54:ca88777b295a 238 // mag_z = -magval[2]/magbias[3];
cocorlow 54:ca88777b295a 239 //
cocorlow 54:ca88777b295a 240 // float lpc_acc = 0.15;
cocorlow 54:ca88777b295a 241 // LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
cocorlow 54:ca88777b295a 242 // LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
cocorlow 54:ca88777b295a 243 // LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;
cocorlow 54:ca88777b295a 244 //
cocorlow 54:ca88777b295a 245 // float lpc_mag = 0.15;
cocorlow 54:ca88777b295a 246 // LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
cocorlow 54:ca88777b295a 247 // LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
cocorlow 54:ca88777b295a 248 // LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
cocorlow 54:ca88777b295a 249 //
cocorlow 54:ca88777b295a 250 // accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
cocorlow 54:ca88777b295a 251 // magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
cocorlow 54:ca88777b295a 252 // LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
cocorlow 54:ca88777b295a 253 // LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
cocorlow 54:ca88777b295a 254 // accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
cocorlow 54:ca88777b295a 255 // magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
cocorlow 54:ca88777b295a 256 // calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
cocorlow 54:ca88777b295a 257 //
cocorlow 54:ca88777b295a 258 //
cocorlow 54:ca88777b295a 259 //}
cocorlow 54:ca88777b295a 260 //
cocorlow 54:ca88777b295a 261 //void updateBetweenMeasures(){
cocorlow 54:ca88777b295a 262 // float q0 = qhat.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 263 // float q1 = qhat.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 264 // float q2 = qhat.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 265 // float q3 = qhat.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 266 //
cocorlow 54:ca88777b295a 267 // Matrix B(4,3);
cocorlow 54:ca88777b295a 268 // B << q1 << q2 << q3
cocorlow 54:ca88777b295a 269 // <<-q0 << q3 <<-q2
cocorlow 54:ca88777b295a 270 // <<-q3 <<-q0 << q1
cocorlow 54:ca88777b295a 271 // << q2 <<-q1 <<-q0;
cocorlow 54:ca88777b295a 272 // B *= 0.5f;
cocorlow 54:ca88777b295a 273 //
cocorlow 54:ca88777b295a 274 // Matrix phi(4,4);
cocorlow 54:ca88777b295a 275 // phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
cocorlow 54:ca88777b295a 276 // << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
cocorlow 54:ca88777b295a 277 // << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
cocorlow 54:ca88777b295a 278 // << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
cocorlow 54:ca88777b295a 279 //
cocorlow 54:ca88777b295a 280 // qhat = phi*qhat;
cocorlow 54:ca88777b295a 281 // float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 54:ca88777b295a 282 // qhat *= (1.0f/ qnorm);
cocorlow 54:ca88777b295a 283 //
cocorlow 54:ca88777b295a 284 // qhat_gyro = phi*qhat_gyro;
cocorlow 54:ca88777b295a 285 // qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
cocorlow 54:ca88777b295a 286 // qhat_gyro *= (1.0f/ qnorm);
cocorlow 54:ca88777b295a 287 //
cocorlow 54:ca88777b295a 288 // Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
cocorlow 54:ca88777b295a 289 //
cocorlow 54:ca88777b295a 290 // q0 = qhat.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 291 // q1 = qhat.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 292 // q2 = qhat.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 293 // q3 = qhat.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 294 //
cocorlow 54:ca88777b295a 295 // D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 296 // D.add(1,2,2*(q1*q2 + q0*q3));
cocorlow 54:ca88777b295a 297 // D.add(1,3,2*(q1*q3 - q0*q2));
cocorlow 54:ca88777b295a 298 // D.add(2,1,2*(q1*q2 - q0*q3));
cocorlow 54:ca88777b295a 299 // D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 300 // D.add(2,3,2*(q2*q3 + q0*q1));
cocorlow 54:ca88777b295a 301 // D.add(3,1,2*(q1*q3 + q0*q2));
cocorlow 54:ca88777b295a 302 // D.add(3,2,2*(q2*q3 - q0*q1));
cocorlow 54:ca88777b295a 303 // D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
cocorlow 54:ca88777b295a 304 //
cocorlow 54:ca88777b295a 305 //}
cocorlow 54:ca88777b295a 306 //
cocorlow 54:ca88777b295a 307 //void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
cocorlow 54:ca88777b295a 308 //
cocorlow 54:ca88777b295a 309 // Matrix u(3,1);
cocorlow 54:ca88777b295a 310 // Matrix v(3,1);
cocorlow 54:ca88777b295a 311 //
cocorlow 54:ca88777b295a 312 // u << u1 << u2 <<u3;
cocorlow 54:ca88777b295a 313 // v << v1 << v2 <<v3;
cocorlow 54:ca88777b295a 314 //
cocorlow 54:ca88777b295a 315 // float q0 = qhat.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 316 // float q1 = qhat.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 317 // float q2 = qhat.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 318 // float q3 = qhat.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 319 //
cocorlow 54:ca88777b295a 320 // Matrix A1(3,3);
cocorlow 54:ca88777b295a 321 // A1 << q0 << q3 << -q2
cocorlow 54:ca88777b295a 322 // <<-q3 << q0 << q1
cocorlow 54:ca88777b295a 323 // <<q2 <<-q1 <<q0;
cocorlow 54:ca88777b295a 324 // A1 *= 2.0f;
cocorlow 54:ca88777b295a 325 //
cocorlow 54:ca88777b295a 326 // Matrix A2(3,3);
cocorlow 54:ca88777b295a 327 // A2 << q1 << q2 << q3
cocorlow 54:ca88777b295a 328 // << q2 <<-q1 << q0
cocorlow 54:ca88777b295a 329 // << q3 <<-q0 <<-q1;
cocorlow 54:ca88777b295a 330 // A2 *= 2.0f;
cocorlow 54:ca88777b295a 331 //
cocorlow 54:ca88777b295a 332 // Matrix A3(3,3);
cocorlow 54:ca88777b295a 333 // A3 <<-q2 << q1 <<-q0
cocorlow 54:ca88777b295a 334 // << q1 << q2 << q3
cocorlow 54:ca88777b295a 335 // << q0 << q3 <<-q2;
cocorlow 54:ca88777b295a 336 // A3 *= 2.0f;
cocorlow 54:ca88777b295a 337 //
cocorlow 54:ca88777b295a 338 // Matrix A4(3,3);
cocorlow 54:ca88777b295a 339 // A4 <<-q3 << q0 << q1
cocorlow 54:ca88777b295a 340 // <<-q0 <<-q3 << q2
cocorlow 54:ca88777b295a 341 // << q1 << q2 << q3;
cocorlow 54:ca88777b295a 342 // A4 *= 2.0f;
cocorlow 54:ca88777b295a 343 //
cocorlow 54:ca88777b295a 344 // Matrix H(3,4);
cocorlow 54:ca88777b295a 345 //
cocorlow 54:ca88777b295a 346 // Matrix ab1(A1*u);
cocorlow 54:ca88777b295a 347 // Matrix ab2(A2*u);
cocorlow 54:ca88777b295a 348 // Matrix ab3(A3*u);
cocorlow 54:ca88777b295a 349 // Matrix ab4(A4*u);
cocorlow 54:ca88777b295a 350 //
cocorlow 54:ca88777b295a 351 // H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
cocorlow 54:ca88777b295a 352 // << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
cocorlow 54:ca88777b295a 353 // << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 354 //
cocorlow 54:ca88777b295a 355 //
cocorlow 54:ca88777b295a 356 // Matrix K(4,3);
cocorlow 54:ca88777b295a 357 // K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
cocorlow 54:ca88777b295a 358 //
cocorlow 54:ca88777b295a 359 // Matrix dq(4,1);
cocorlow 54:ca88777b295a 360 // dq = K*(v-D*u);
cocorlow 54:ca88777b295a 361 // qhat = qhat+dq;
cocorlow 54:ca88777b295a 362 //
cocorlow 54:ca88777b295a 363 // float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 54:ca88777b295a 364 // qhat *= (1.0f/ qnorm);
cocorlow 54:ca88777b295a 365 //
cocorlow 54:ca88777b295a 366 // Matrix eye4(4,4);
cocorlow 54:ca88777b295a 367 // eye4 << 1 << 0 << 0 << 0
cocorlow 54:ca88777b295a 368 // << 0 << 1 << 0 << 0
cocorlow 54:ca88777b295a 369 // << 0 << 0 << 1 << 0
cocorlow 54:ca88777b295a 370 // << 0 << 0 << 0 << 1;
cocorlow 54:ca88777b295a 371 // Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
cocorlow 54:ca88777b295a 372 //}
cocorlow 54:ca88777b295a 373 //
cocorlow 54:ca88777b295a 374 //void computeAngles()
cocorlow 54:ca88777b295a 375 //{
cocorlow 54:ca88777b295a 376 // float q0 = qhat.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 377 // float q1 = qhat.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 378 // float q2 = qhat.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 379 // float q3 = qhat.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 380 // roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
cocorlow 54:ca88777b295a 381 // pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
cocorlow 54:ca88777b295a 382 // yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 383 //
cocorlow 54:ca88777b295a 384 // q0 = qhat_gyro.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 385 // q1 = qhat_gyro.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 386 // q2 = qhat_gyro.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 387 // q3 = qhat_gyro.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 388 // roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
cocorlow 54:ca88777b295a 389 // pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
cocorlow 54:ca88777b295a 390 // yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 391 //
cocorlow 54:ca88777b295a 392 //}
cocorlow 54:ca88777b295a 393 //
cocorlow 54:ca88777b295a 394 //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){
cocorlow 54:ca88777b295a 395 // Matrix W1(3,1);
cocorlow 54:ca88777b295a 396 // W1 << fb1 << fb2<< fb3;
cocorlow 54:ca88777b295a 397 // Matrix W2(3,1);
cocorlow 54:ca88777b295a 398 // W2 << mb1 << mb2<< mb3;
cocorlow 54:ca88777b295a 399 //
cocorlow 54:ca88777b295a 400 // Matrix V1(3,1);
cocorlow 54:ca88777b295a 401 // V1 << fn1 << fn2<< fn3;
cocorlow 54:ca88777b295a 402 // Matrix V2(3,1);
cocorlow 54:ca88777b295a 403 // V2 << mn1 << mn2<< mn3;
cocorlow 54:ca88777b295a 404 //
cocorlow 54:ca88777b295a 405 //
cocorlow 54:ca88777b295a 406 // Matrix Ou2(3,1);
cocorlow 54:ca88777b295a 407 // 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 );
cocorlow 54:ca88777b295a 408 // Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
cocorlow 54:ca88777b295a 409 // Matrix Ou3(3,1);
cocorlow 54:ca88777b295a 410 // 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 );
cocorlow 54:ca88777b295a 411 // Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
cocorlow 54:ca88777b295a 412 // Matrix R2(3,1);
cocorlow 54:ca88777b295a 413 // 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 );
cocorlow 54:ca88777b295a 414 // R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
cocorlow 54:ca88777b295a 415 // Matrix R3(3,1);
cocorlow 54:ca88777b295a 416 // 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 );
cocorlow 54:ca88777b295a 417 // R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
cocorlow 54:ca88777b295a 418 //
cocorlow 54:ca88777b295a 419 // Matrix Mou(3,3);
cocorlow 54:ca88777b295a 420 // Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
cocorlow 54:ca88777b295a 421 // << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
cocorlow 54:ca88777b295a 422 // << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 423 // Matrix Mr(3,3);
cocorlow 54:ca88777b295a 424 // Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
cocorlow 54:ca88777b295a 425 // << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
cocorlow 54:ca88777b295a 426 // << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 427 //
cocorlow 54:ca88777b295a 428 // Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
cocorlow 54:ca88777b295a 429 //
cocorlow 54:ca88777b295a 430 // float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
cocorlow 54:ca88777b295a 431 //
cocorlow 54:ca88777b295a 432 // qhat.add(1,1,0.5*sqtrp1);
cocorlow 54:ca88777b295a 433 // qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
cocorlow 54:ca88777b295a 434 // qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
cocorlow 54:ca88777b295a 435 // qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
cocorlow 54:ca88777b295a 436 //
cocorlow 54:ca88777b295a 437 // float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
cocorlow 54:ca88777b295a 438 // qhat *= (1.0f/ qnorm);
cocorlow 54:ca88777b295a 439 //
cocorlow 54:ca88777b295a 440 // qhat_gyro = qhat;
cocorlow 54:ca88777b295a 441 //
cocorlow 54:ca88777b295a 442 // float q0 = qhat.getNumber( 1, 1 );
cocorlow 54:ca88777b295a 443 // float q1 = qhat.getNumber( 2, 1 );
cocorlow 54:ca88777b295a 444 // float q2 = qhat.getNumber( 3, 1 );
cocorlow 54:ca88777b295a 445 // float q3 = qhat.getNumber( 4, 1 );
cocorlow 54:ca88777b295a 446 //
cocorlow 54:ca88777b295a 447 // D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 448 // D.add(1,2,2*(q1*q2 + q0*q3));
cocorlow 54:ca88777b295a 449 // D.add(1,3,2*(q1*q3 - q0*q2));
cocorlow 54:ca88777b295a 450 // D.add(2,1,2*(q1*q2 - q0*q3));
cocorlow 54:ca88777b295a 451 // D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
cocorlow 54:ca88777b295a 452 // D.add(2,3,2*(q2*q3 + q0*q1));
cocorlow 54:ca88777b295a 453 // D.add(3,1,2*(q1*q3 + q0*q2));
cocorlow 54:ca88777b295a 454 // D.add(3,2,2*(q2*q3 - q0*q1));
cocorlow 54:ca88777b295a 455 // D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
cocorlow 54:ca88777b295a 456 //
cocorlow 54:ca88777b295a 457 //}
cocorlow 54:ca88777b295a 458 //
cocorlow 54:ca88777b295a 459 //void calcDynAcc(){
cocorlow 54:ca88777b295a 460 // dynacc_x = LPacc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]);
cocorlow 54:ca88777b295a 461 // dynacc_y = LPacc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]);
cocorlow 54:ca88777b295a 462 // dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
cocorlow 54:ca88777b295a 463 //}
cocorlow 54:ca88777b295a 464 //
cocorlow 54:ca88777b295a 465 //// 割り込まれた時点での出力(computeの結果）を返す関数
cocorlow 54:ca88777b295a 466 //void calcServoOut(){
cocorlow 54:ca88777b295a 467 // if(sbus.failSafe == false) {
cocorlow 54:ca88777b295a 468 // // sbusデータの読み込み
cocorlow 54:ca88777b295a 469 // for (int i =0 ; i < 16;i ++){
cocorlow 54:ca88777b295a 470 // rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
cocorlow 54:ca88777b295a 471 // }
cocorlow 54:ca88777b295a 472 // }
cocorlow 54:ca88777b295a 473 // pitchPID.setGain(6.36, 10.6,0.0);
cocorlow 54:ca88777b295a 474 // pitchratePID.setGain(0.9540,0.0,0.0);
cocorlow 54:ca88777b295a 475 // pitchPID.setProcessValue(pitch);
cocorlow 54:ca88777b295a 476 // pitchratePID.setProcessValue(gyro_y);
cocorlow 54:ca88777b295a 477 // float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
cocorlow 54:ca88777b295a 478 //
cocorlow 54:ca88777b295a 479 // scaledServoOut[0]=de;
cocorlow 54:ca88777b295a 480 //
cocorlow 54:ca88777b295a 481 // float LP_servo = 0.2;
cocorlow 54:ca88777b295a 482 // for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
cocorlow 54:ca88777b295a 483 // servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
cocorlow 54:ca88777b295a 484 // if(servoOut[i]<servoPwmMin) {
cocorlow 54:ca88777b295a 485 // servoOut[i] = servoPwmMin;
cocorlow 54:ca88777b295a 486 // };
cocorlow 54:ca88777b295a 487 // if(servoOut[i]>servoPwmMax) {
cocorlow 54:ca88777b295a 488 // servoOut[i] = servoPwmMax;
cocorlow 54:ca88777b295a 489 // };
cocorlow 54:ca88777b295a 490 // }
cocorlow 54:ca88777b295a 491 //
cocorlow 54:ca88777b295a 492 // servo.pulsewidth_us(servoOut[0]);
cocorlow 54:ca88777b295a 493 //
cocorlow 54:ca88777b295a 494 // //観測アップデート
cocorlow 54:ca88777b295a 495 // calcDynAcc();
cocorlow 54:ca88777b295a 496 // th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
cocorlow 54:ca88777b295a 497 // dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
cocorlow 54:ca88777b295a 498 // accnormerr = abs(LPaccnorm-accrefnorm);
cocorlow 54:ca88777b295a 499 // //静止時100個の平均　0.01877522 0.00514146 0.00477393
cocorlow 54:ca88777b295a 500 //
cocorlow 54:ca88777b295a 501 // //int ang_th = th_mg < 0.01877522;
cocorlow 54:ca88777b295a 502 // //int dyn_th = dynaccnorm < 0.00514146;
cocorlow 54:ca88777b295a 503 // //int norm_th = accnormerr< 0.00477393;
cocorlow 54:ca88777b295a 504 // int ang_th = th_mg < 0.01877522/5.0;
cocorlow 54:ca88777b295a 505 // int dyn_th = dynaccnorm < 0.00514146/5.0;
cocorlow 54:ca88777b295a 506 // int norm_th = accnormerr< 0.00477393/5.0;
cocorlow 54:ca88777b295a 507 // if(dyn_th+ang_th+norm_th>0){
cocorlow 54:ca88777b295a 508 // //if(dyn_th+ang_th+norm_th>-1){
cocorlow 54:ca88777b295a 509 // obs_count += 1;
cocorlow 54:ca88777b295a 510 // updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
cocorlow 54:ca88777b295a 511 // updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
cocorlow 54:ca88777b295a 512 // }
cocorlow 54:ca88777b295a 513 //
cocorlow 54:ca88777b295a 514 // if(loop_count >= 10){
cocorlow 54:ca88777b295a 515 // writeSdcard();
cocorlow 54:ca88777b295a 516 // loop_count = 1;
cocorlow 54:ca88777b295a 517 // obs_count = 0;
cocorlow 54:ca88777b295a 518 // }else{
cocorlow 54:ca88777b295a 519 // loop_count +=1;
cocorlow 54:ca88777b295a 520 // }
cocorlow 54:ca88777b295a 521 //
cocorlow 54:ca88777b295a 522 //
cocorlow 54:ca88777b295a 523 //}
cocorlow 54:ca88777b295a 524 //
cocorlow 54:ca88777b295a 525 //int main()
cocorlow 54:ca88777b295a 526 //{
cocorlow 54:ca88777b295a 527 // pitchPID.setSetPoint(0.0);
cocorlow 54:ca88777b295a 528 // pitchratePID.setSetPoint(0.0);
cocorlow 54:ca88777b295a 529 // pitchPID.setBias(0.0);
cocorlow 54:ca88777b295a 530 // pitchratePID.setBias(0.0);
cocorlow 54:ca88777b295a 531 // pitchPID.setOutputLimits(-1.0,1.0);
cocorlow 54:ca88777b295a 532 // pitchratePID.setOutputLimits(-1.0,1.0);
cocorlow 54:ca88777b295a 533 // pitchPID.setInputLimits(-pi,pi);
cocorlow 54:ca88777b295a 534 // pitchratePID.setInputLimits(-pi,pi);
cocorlow 54:ca88777b295a 535 //
cocorlow 54:ca88777b295a 536 // servo.period_us(15000.0);
cocorlow 54:ca88777b295a 537 // servo.pulsewidth_us(1500.0);
cocorlow 54:ca88777b295a 538 //
cocorlow 54:ca88777b295a 539 //// pc.baud(57600);
cocorlow 54:ca88777b295a 540 // accelgyro.initialize();
cocorlow 54:ca88777b295a 541 // //加速度計のフルスケールレンジを設定
cocorlow 54:ca88777b295a 542 // accelgyro.setFullScaleAccelRange(ACCEL_FSR);
cocorlow 54:ca88777b295a 543 // //角速度計のフルスケールレンジを設定
cocorlow 54:ca88777b295a 544 // accelgyro.setFullScaleGyroRange(GYRO_FSR);
cocorlow 54:ca88777b295a 545 // //MPU6050のLPFを設定
cocorlow 54:ca88777b295a 546 // accelgyro.setDLPFMode(MPU6050_LPF);
cocorlow 54:ca88777b295a 547 // //地磁気
cocorlow 54:ca88777b295a 548 // mag.enable();
cocorlow 54:ca88777b295a 549 //
cocorlow 54:ca88777b295a 550 // qhat << 1 << 0 << 0 << 0;
cocorlow 54:ca88777b295a 551 //
cocorlow 54:ca88777b295a 552 // D.add(1,1,1.0);
cocorlow 54:ca88777b295a 553 // D.add(2,2,1.0);
cocorlow 54:ca88777b295a 554 // D.add(3,3,1.0);
cocorlow 54:ca88777b295a 555 //
cocorlow 54:ca88777b295a 556 // Phat.add(1,1,0.05);
cocorlow 54:ca88777b295a 557 // Phat.add(2,2,0.05);
cocorlow 54:ca88777b295a 558 // Phat.add(3,3,0.05);
cocorlow 54:ca88777b295a 559 // Phat.add(4,4,0.05);
cocorlow 54:ca88777b295a 560 //
cocorlow 54:ca88777b295a 561 // Qgyro.add(1,1,0.0224);
cocorlow 54:ca88777b295a 562 // Qgyro.add(2,2,0.0224);
cocorlow 54:ca88777b295a 563 // Qgyro.add(3,3,0.0224);
cocorlow 54:ca88777b295a 564 //
cocorlow 54:ca88777b295a 565 // Racc.add(1,1,0.0330*200);
cocorlow 54:ca88777b295a 566 // Racc.add(2,2,0.0330*200);
cocorlow 54:ca88777b295a 567 // Racc.add(3,3,0.0330*200);
cocorlow 54:ca88777b295a 568 //
cocorlow 54:ca88777b295a 569 // Rmag.add(1,1,1.0);
cocorlow 54:ca88777b295a 570 // Rmag.add(2,2,1.0);
cocorlow 54:ca88777b295a 571 // Rmag.add(3,3,1.0);
cocorlow 54:ca88777b295a 572 //
cocorlow 54:ca88777b295a 573 // calibrationFlag = userButton.read();
cocorlow 54:ca88777b295a 574 // if(calibrationFlag == 0){
cocorlow 54:ca88777b295a 575 //
cocorlow 54:ca88777b295a 576 // pc.printf("reading calibration value\r\n");
cocorlow 54:ca88777b295a 577 // //キャリブレーション値を取得
cocorlow 54:ca88777b295a 578 // U read_calib;
cocorlow 54:ca88777b295a 579 // readEEPROM(address, pointeraddress, read_calib.c, N_EEPROM*4);
cocorlow 54:ca88777b295a 580 // wait(3);
cocorlow 54:ca88777b295a 581 // pos_tail = read_calib.i[0];
cocorlow 54:ca88777b295a 582 // agoffset[3] = float(read_calib.i[5]);
cocorlow 54:ca88777b295a 583 // agoffset[4] = float(read_calib.i[6]);
cocorlow 54:ca88777b295a 584 // agoffset[5] = float(read_calib.i[7]);
cocorlow 54:ca88777b295a 585 // magbias[0] = float(read_calib.i[1])/1000.0;
cocorlow 54:ca88777b295a 586 // magbias[1] = float(read_calib.i[2])/1000.0;
cocorlow 54:ca88777b295a 587 // magbias[2] = float(read_calib.i[3])/1000.0;
cocorlow 54:ca88777b295a 588 // magbias[3] = float(read_calib.i[4])/1000.0;
cocorlow 54:ca88777b295a 589 // pitch_align = float(read_calib.i[8])/200000.0;
cocorlow 54:ca88777b295a 590 // roll_align = float(read_calib.i[9])/200000.0;
cocorlow 54:ca88777b295a 591 // tail_address = (int)read_calib.i[10];
cocorlow 54:ca88777b295a 592 //
cocorlow 54:ca88777b295a 593 // switch(pos_tail){
cocorlow 54:ca88777b295a 594 // case 1:
cocorlow 54:ca88777b295a 595 // pc.printf("This MBED is Located at Left \r\n");
cocorlow 54:ca88777b295a 596 // break;
cocorlow 54:ca88777b295a 597 // case 2:
cocorlow 54:ca88777b295a 598 // pc.printf("This MBED is Located at Center \r\n");
cocorlow 54:ca88777b295a 599 // break;
cocorlow 54:ca88777b295a 600 // case 3:
cocorlow 54:ca88777b295a 601 // pc.printf("This MBED is Located at Right \r\n");
cocorlow 54:ca88777b295a 602 // break;
cocorlow 54:ca88777b295a 603 // default: // error situation
cocorlow 54:ca88777b295a 604 // pc.printf("error\r\n");
cocorlow 54:ca88777b295a 605 // break;
cocorlow 54:ca88777b295a 606 // }
cocorlow 54:ca88777b295a 607 // pc.printf("tail_address : %d\r\n", tail_address);
cocorlow 54:ca88777b295a 608 // pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",pitch_align*180.0/pi,roll_align*180.0/pi);
cocorlow 54:ca88777b295a 609 // getIMUval();
cocorlow 54:ca88777b295a 610 // 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);
cocorlow 54:ca88777b295a 611 // float sumLPaccnorm = 0;
cocorlow 54:ca88777b295a 612 // for(int i = 0; i<1000 ;i++){
cocorlow 54:ca88777b295a 613 // getIMUval();
cocorlow 54:ca88777b295a 614 // val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
cocorlow 54:ca88777b295a 615 // sumLPaccnorm += LPaccnorm;
cocorlow 54:ca88777b295a 616 // }
cocorlow 54:ca88777b295a 617 // accref[2]=-sumLPaccnorm/1000;
cocorlow 54:ca88777b295a 618 // val_thmg /= 1000;
cocorlow 54:ca88777b295a 619 // LoopTicker PIDtick;
cocorlow 54:ca88777b295a 620 // PIDtick.attach(calcServoOut,PID_dt);
cocorlow 54:ca88777b295a 621 //
cocorlow 54:ca88777b295a 622 // t.start();
cocorlow 54:ca88777b295a 623 //
cocorlow 54:ca88777b295a 624 // while(1) {
cocorlow 54:ca88777b295a 625 // float tstart = t.read();
cocorlow 54:ca88777b295a 626 // //姿勢角を更新
cocorlow 54:ca88777b295a 627 // getIMUval();
cocorlow 54:ca88777b295a 628 // updateBetweenMeasures();
cocorlow 54:ca88777b295a 629 // computeAngles();
cocorlow 54:ca88777b295a 630 // PIDtick.loop();
cocorlow 54:ca88777b295a 631 //
cocorlow 54:ca88777b295a 632 // float tend = t.read();
cocorlow 54:ca88777b295a 633 // att_dt = (tend-tstart);
cocorlow 54:ca88777b295a 634 // }
cocorlow 54:ca88777b295a 635 // }else{
cocorlow 54:ca88777b295a 636 // pc.printf("\r\nEnter to Calibration Mode\r\n");
cocorlow 54:ca88777b295a 637 // wait(5);
cocorlow 54:ca88777b295a 638 // pc.printf("Acc and Gyro Calibration Start\r\n");
cocorlow 54:ca88777b295a 639 //
cocorlow 54:ca88777b295a 640 // int gxs = 0;
cocorlow 54:ca88777b295a 641 // int gys = 0;
cocorlow 54:ca88777b295a 642 // int gzs = 0;
cocorlow 54:ca88777b295a 643 // int iter_n = 10000;
cocorlow 54:ca88777b295a 644 // for(int i = 0;i<iter_n ;i++){
cocorlow 54:ca88777b295a 645 // accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
cocorlow 54:ca88777b295a 646 // gxs += gx;
cocorlow 54:ca88777b295a 647 // gys += gy;
cocorlow 54:ca88777b295a 648 // gzs -= gz;
cocorlow 54:ca88777b295a 649 // //wait(0.01);
cocorlow 54:ca88777b295a 650 // }
cocorlow 54:ca88777b295a 651 // gxs = gxs /iter_n;
cocorlow 54:ca88777b295a 652 // gys = gys /iter_n;
cocorlow 54:ca88777b295a 653 // gzs = gzs /iter_n;
cocorlow 54:ca88777b295a 654 // agoffset[3] = gxs;
cocorlow 54:ca88777b295a 655 // agoffset[4] = gys;
cocorlow 54:ca88777b295a 656 // agoffset[5] = gzs;
cocorlow 54:ca88777b295a 657 // pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
cocorlow 54:ca88777b295a 658 //
cocorlow 54:ca88777b295a 659 // pc.printf("Mag Calibration Start\r\n");
cocorlow 54:ca88777b295a 660 // float f = 0;
cocorlow 54:ca88777b295a 661 // while(1){
cocorlow 54:ca88777b295a 662 // mag.getAxis(mdata); // flush the magnetmeter
cocorlow 54:ca88777b295a 663 // magval[0] = (mdata.x - magbias[0]);
cocorlow 54:ca88777b295a 664 // magval[1] = (mdata.y - magbias[1]);
cocorlow 54:ca88777b295a 665 // magval[2] = (mdata.z - magbias[2]);
cocorlow 54:ca88777b295a 666 // float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
cocorlow 54:ca88777b295a 667 // float lr = 0.00001f;
cocorlow 54:ca88777b295a 668 // f = mag_r - magbias[3]*magbias[3];
cocorlow 54:ca88777b295a 669 // magbias[0] = magbias[0] + 4 * lr * f * magval[0];
cocorlow 54:ca88777b295a 670 // magbias[1] = magbias[1] + 4 * lr * f * magval[1];
cocorlow 54:ca88777b295a 671 // magbias[2] = magbias[2] + 4 * lr * f * magval[2];
cocorlow 54:ca88777b295a 672 // magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
cocorlow 54:ca88777b295a 673 //
cocorlow 54:ca88777b295a 674 // if(userButton.read() == 1){
cocorlow 54:ca88777b295a 675 // break;
cocorlow 54:ca88777b295a 676 // }
cocorlow 54:ca88777b295a 677 // wait(0.001);
cocorlow 54:ca88777b295a 678 // }
cocorlow 54:ca88777b295a 679 // pc.printf("Magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
cocorlow 54:ca88777b295a 680 //
cocorlow 54:ca88777b295a 681 // pc.printf("Determine Position of MBED\r\n");
cocorlow 54:ca88777b295a 682 // wait(1);
cocorlow 54:ca88777b295a 683 // pc.printf("Press the user button\r\n");
cocorlow 54:ca88777b295a 684 //
cocorlow 54:ca88777b295a 685 // while(userButton.read() == 0){wait(0.01);};
cocorlow 54:ca88777b295a 686 // while(1){
cocorlow 54:ca88777b295a 687 // pc.printf("Left\r\n");
cocorlow 54:ca88777b295a 688 // wait(2);
cocorlow 54:ca88777b295a 689 // if(userButton.read() == 0){
cocorlow 54:ca88777b295a 690 // pos_tail=1;
cocorlow 54:ca88777b295a 691 // tail_address = 1111;
cocorlow 54:ca88777b295a 692 // break;
cocorlow 54:ca88777b295a 693 // };
cocorlow 54:ca88777b295a 694 // pc.printf("Center\r\n");
cocorlow 54:ca88777b295a 695 // wait(2);
cocorlow 54:ca88777b295a 696 // if(userButton.read() == 0){
cocorlow 54:ca88777b295a 697 // pos_tail=2;
cocorlow 54:ca88777b295a 698 // tail_address = 2222;
cocorlow 54:ca88777b295a 699 // break;
cocorlow 54:ca88777b295a 700 // };
cocorlow 54:ca88777b295a 701 // pc.printf("Right\r\n");
cocorlow 54:ca88777b295a 702 // wait(2);
cocorlow 54:ca88777b295a 703 // if(userButton.read() == 0){
cocorlow 54:ca88777b295a 704 // pos_tail=3;
cocorlow 54:ca88777b295a 705 // tail_address = 3333;
cocorlow 54:ca88777b295a 706 // break;
cocorlow 54:ca88777b295a 707 // };
cocorlow 54:ca88777b295a 708 // };
cocorlow 54:ca88777b295a 709 // pc.printf("tail_address : %d\r\n", tail_address);
cocorlow 54:ca88777b295a 710 // switch(pos_tail){
cocorlow 54:ca88777b295a 711 // case 1:
cocorlow 54:ca88777b295a 712 // pc.printf("This MBED is Located at Left \r\n");
cocorlow 54:ca88777b295a 713 // break;
cocorlow 54:ca88777b295a 714 // case 2:
cocorlow 54:ca88777b295a 715 // pc.printf("This MBED is Located at Center \r\n");
cocorlow 54:ca88777b295a 716 // break;
cocorlow 54:ca88777b295a 717 // case 3:
cocorlow 54:ca88777b295a 718 // pc.printf("This MBED is Located at Right \r\n");
cocorlow 54:ca88777b295a 719 // break;
cocorlow 54:ca88777b295a 720 // default: // error situation
cocorlow 54:ca88777b295a 721 // pc.printf("error\r\n");
cocorlow 54:ca88777b295a 722 // break;
cocorlow 54:ca88777b295a 723 // }
cocorlow 54:ca88777b295a 724 //
cocorlow 54:ca88777b295a 725 // //姿勢オフセットを計算
cocorlow 54:ca88777b295a 726 // pitch_align = 0.0*3.141562/180.0;
cocorlow 54:ca88777b295a 727 // roll_align = 0.0*3.141562/180.0;
cocorlow 54:ca88777b295a 728 // float ave_pitch = 0.0;
cocorlow 54:ca88777b295a 729 // float ave_roll = 0.0;
cocorlow 54:ca88777b295a 730 // for (int i = 0 ; i<2200; i++){
cocorlow 54:ca88777b295a 731 // float tstart = t.read();
cocorlow 54:ca88777b295a 732 // //姿勢角を更新
cocorlow 54:ca88777b295a 733 // getIMUval();
cocorlow 54:ca88777b295a 734 // updateBetweenMeasures();
cocorlow 54:ca88777b295a 735 // updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
cocorlow 54:ca88777b295a 736 // updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
cocorlow 54:ca88777b295a 737 // computeAngles();
cocorlow 54:ca88777b295a 738 // if(i>199){
cocorlow 54:ca88777b295a 739 // ave_pitch += pitch;
cocorlow 54:ca88777b295a 740 // ave_roll += roll;
cocorlow 54:ca88777b295a 741 // }
cocorlow 54:ca88777b295a 742 // wait(0.001);
cocorlow 54:ca88777b295a 743 // float tend = t.read();
cocorlow 54:ca88777b295a 744 // att_dt = (tend-tstart);
cocorlow 54:ca88777b295a 745 // }
cocorlow 54:ca88777b295a 746 //
cocorlow 54:ca88777b295a 747 // 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);
cocorlow 54:ca88777b295a 748 //
cocorlow 54:ca88777b295a 749 // pc.printf("Writing to EEPROM...\r\n");
cocorlow 54:ca88777b295a 750 // U transfer_data;
cocorlow 54:ca88777b295a 751 // transfer_data.i[0] = pos_tail;
cocorlow 54:ca88777b295a 752 // for (int i = 1; i < 5; i++) {
cocorlow 54:ca88777b295a 753 // if (!isnan(magbias[i - 1]))
cocorlow 54:ca88777b295a 754 // transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。
cocorlow 54:ca88777b295a 755 // else {
cocorlow 54:ca88777b295a 756 // pc.printf("Mag bias is NOT transferred\n");
cocorlow 54:ca88777b295a 757 // transfer_data.i[i] = 100;
cocorlow 54:ca88777b295a 758 // }
cocorlow 54:ca88777b295a 759 // }
cocorlow 54:ca88777b295a 760 // // gxs,gys,gzsを送る
cocorlow 54:ca88777b295a 761 // int gxyzs[3] = {gxs, gys, gzs};
cocorlow 54:ca88777b295a 762 // for (int i = 5; i < 8; i++) {
cocorlow 54:ca88777b295a 763 // if (!isnan(gxyzs[i - 5]))
cocorlow 54:ca88777b295a 764 // transfer_data.i[i] = gxyzs[i - 5];
cocorlow 54:ca88777b295a 765 // else {
cocorlow 54:ca88777b295a 766 // pc.printf("gxyzs is NOT transferred\n");
cocorlow 54:ca88777b295a 767 // transfer_data.i[i] = 0;
cocorlow 54:ca88777b295a 768 // }
cocorlow 54:ca88777b295a 769 // }
cocorlow 54:ca88777b295a 770 //
cocorlow 54:ca88777b295a 771 // // ave_pitch,ave_rollを送る
cocorlow 54:ca88777b295a 772 // int ave_pr[2] = {ave_pitch*100, ave_roll*100};
cocorlow 54:ca88777b295a 773 // for (int i = 8; i < 10; i++) {
cocorlow 54:ca88777b295a 774 // if (!isnan(ave_pr[i - 8]))
cocorlow 54:ca88777b295a 775 // transfer_data.i[i] = ave_pr[i - 8];
cocorlow 54:ca88777b295a 776 // else {
cocorlow 54:ca88777b295a 777 // pc.printf("alignment data is NOT transferred\n");
cocorlow 54:ca88777b295a 778 // transfer_data.i[i] = 0;
cocorlow 54:ca88777b295a 779 // }
cocorlow 54:ca88777b295a 780 // }
cocorlow 54:ca88777b295a 781 // transfer_data.i[10] = tail_address;
cocorlow 54:ca88777b295a 782 // for (int i = 0; i < N_EEPROM; i++) {
cocorlow 54:ca88777b295a 783 // pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]);
cocorlow 54:ca88777b295a 784 // }
cocorlow 54:ca88777b295a 785 // writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4);
cocorlow 54:ca88777b295a 786 // wait(3);
cocorlow 54:ca88777b295a 787 //
cocorlow 54:ca88777b295a 788 // U read_test;
cocorlow 54:ca88777b295a 789 // readEEPROM(address, pointeraddress, read_test.c, N_EEPROM*4);
cocorlow 54:ca88777b295a 790 // wait(3);
cocorlow 54:ca88777b295a 791 // for (int i = 0 ; i < N_EEPROM; i ++){
cocorlow 54:ca88777b295a 792 // pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]);
cocorlow 54:ca88777b295a 793 // }
cocorlow 54:ca88777b295a 794 //
cocorlow 54:ca88777b295a 795 // while(1) {wait(1000);}
cocorlow 54:ca88777b295a 796 // }
cocorlow 54:ca88777b295a 797 //}