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@37:dad55cf05e3d, 2021-03-03 (annotated)

Committer:
NaotoMorita
Date:
Wed Mar 03 09:32:10 2021 +0000
Revision:
37:dad55cf05e3d
Parent:
36:e68ce293306e
Child:
38:acc7cdcf56dd
quaternion dekita

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 0:6b18a09a6628 1 #include "mbed.h"
naker 20:2c3f113a8e8f 2 #include "PIDcontroller.h"
NaotoMorita 0:6b18a09a6628 3 #include "SBUS.hpp"
NaotoMorita 36:e68ce293306e 4 //#include <MadgwickAHRS.h>
cocorlow 6:2cba569272fe 5 #include "LoopTicker.hpp"
naker 15:6abaac6e5b03 6 #include "MPU6050.h"
NaotoMorita 17:6b7a363d06d2 7 #include <I2Cdev.h>
NaotoMorita 36:e68ce293306e 8 #include "FastPWM.h"
NaotoMorita 36:e68ce293306e 9 #include "Matrix.h"
NaotoMorita 36:e68ce293306e 10 #include "MatrixMath.h"
NaotoMorita 36:e68ce293306e 11 #include <math.h>
cocorlow 6:2cba569272fe 12
NaotoMorita 17:6b7a363d06d2 13 #define MPU6050_PWR_MGMT_1 0x6B
NaotoMorita 17:6b7a363d06d2 14 #define MPU_ADDRESS 0x68
NaotoMorita 17:6b7a363d06d2 15 #define pi 3.141562
NaotoMorita 17:6b7a363d06d2 16 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 17:6b7a363d06d2 17 #define ACCEL_SSF 36500.0
NaotoMorita 17:6b7a363d06d2 18 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 19 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 20 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 27:43bd0e444633 21
NaotoMorita 17:6b7a363d06d2 22 MPU6050 accelgyro;
NaotoMorita 36:e68ce293306e 23 //Madgwick MadgwickFilter;
NaotoMorita 0:6b18a09a6628 24 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 25 Serial pc(USBTX, USBRX);
naker 26:62d9857eaecc 26 Serial sd(PG_14,PG_9);
cocorlow 3:79e62f9b13c8 27 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 28 DigitalOut led3(LED3);
NaotoMorita 27:43bd0e444633 29
NaotoMorita 36:e68ce293306e 30 FastPWM servoRight(PE_9);
NaotoMorita 36:e68ce293306e 31 FastPWM servoLeft(PE_11);
NaotoMorita 36:e68ce293306e 32 FastPWM servoThrust(PA_0);
NaotoMorita 36:e68ce293306e 33 const double PID_dt = 0.015;
NaotoMorita 36:e68ce293306e 34 PID pitchPID(5.0, 0,0,PID_dt); //rad
NaotoMorita 31:8d02f3b9a067 35 PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 36:e68ce293306e 36 PID rollPID(5.0,0.0,0.0,PID_dt);
NaotoMorita 31:8d02f3b9a067 37 PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 38 Timer t;
cocorlow 3:79e62f9b13c8 39
NaotoMorita 36:e68ce293306e 40 Matrix qhat(4,1);
NaotoMorita 36:e68ce293306e 41 Matrix Phat(4,4);
NaotoMorita 37:dad55cf05e3d 42 Matrix Qgyro(3,3);
NaotoMorita 36:e68ce293306e 43 Matrix Racc(3,3);
NaotoMorita 36:e68ce293306e 44
NaotoMorita 36:e68ce293306e 45
NaotoMorita 36:e68ce293306e 46 int loop_count = 1;
NaotoMorita 36:e68ce293306e 47 float att_dt = 0.01;
naker 23:4a490fa8bf4a 48 float rc[16];
naker 25:5aca9b224226 49 float roll_ac;
naker 15:6abaac6e5b03 50 double pitch = 0.0;
naker 15:6abaac6e5b03 51 double roll = 0.0;
naker 15:6abaac6e5b03 52 double yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 53 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 54 int16_t gx, gy, gz;
NaotoMorita 17:6b7a363d06d2 55 double acc_x,acc_y,acc_z;
NaotoMorita 17:6b7a363d06d2 56 double gyro_x,gyro_y,gyro_z;
cocorlow 3:79e62f9b13c8 57 int out1, out2;
NaotoMorita 36:e68ce293306e 58 float scaledServoOut[3]= {0,0};
NaotoMorita 28:fc6c46c1db65 59 float scaledMotorOut[1]= {-1};
NaotoMorita 36:e68ce293306e 60 float servoOut[3] = {1500.0,1500.0};
NaotoMorita 36:e68ce293306e 61 float motorOut[1] = {1100.0};
NaotoMorita 36:e68ce293306e 62 float servoPwmMax = 1800.0;
NaotoMorita 36:e68ce293306e 63 float servoPwmMin = 1200.0;
NaotoMorita 36:e68ce293306e 64 float motorPwmMax = 2000.0;
NaotoMorita 36:e68ce293306e 65 float motorPwmMin = 1100.0;
NaotoMorita 36:e68ce293306e 66 int offsetVal[6] = {0,0,0,-600,570,265};
cocorlow 3:79e62f9b13c8 67
NaotoMorita 17:6b7a363d06d2 68 const double pitch_align = 0.0;
NaotoMorita 17:6b7a363d06d2 69 const double roll_align = -0.0;
NaotoMorita 27:43bd0e444633 70
NaotoMorita 27:43bd0e444633 71
NaotoMorita 35:4535af88f7bf 72 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
cocorlow 7:70161eb0f854 73 {
cocorlow 7:70161eb0f854 74 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 75 }
NaotoMorita 27:43bd0e444633 76
NaotoMorita 30:214ddc613a35 77 void writeSdcard()
naker 20:2c3f113a8e8f 78 {
NaotoMorita 36:e68ce293306e 79 pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f);
naker 20:2c3f113a8e8f 80 }
NaotoMorita 27:43bd0e444633 81
naker 22:da9893b71f24 82 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 83 void calcServoOut(){
NaotoMorita 28:fc6c46c1db65 84 if(sbus.failSafe == false) {
NaotoMorita 28:fc6c46c1db65 85 // sbusデータの読み込み
NaotoMorita 28:fc6c46c1db65 86 for (int i =0 ; i < 16;i ++){
NaotoMorita 35:4535af88f7bf 87 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
NaotoMorita 27:43bd0e444633 88 }
NaotoMorita 28:fc6c46c1db65 89 }
NaotoMorita 28:fc6c46c1db65 90
NaotoMorita 36:e68ce293306e 91
NaotoMorita 36:e68ce293306e 92 //pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
NaotoMorita 36:e68ce293306e 93 //roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
NaotoMorita 28:fc6c46c1db65 94 pitchPID.setProcessValue(pitch);
NaotoMorita 28:fc6c46c1db65 95 pitchratePID.setProcessValue(gyro_y);
NaotoMorita 28:fc6c46c1db65 96 rollPID.setProcessValue(roll);
NaotoMorita 28:fc6c46c1db65 97 rollratePID.setProcessValue(gyro_x);
NaotoMorita 36:e68ce293306e 98 float de = -pitchPID.compute()-pitchratePID.compute()-rc[1]+rc[0];
NaotoMorita 36:e68ce293306e 99 float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
NaotoMorita 28:fc6c46c1db65 100 float dT = rc[2];
NaotoMorita 28:fc6c46c1db65 101
NaotoMorita 28:fc6c46c1db65 102 scaledServoOut[0]=de+da;
NaotoMorita 36:e68ce293306e 103 scaledServoOut[1]=-de+da;
NaotoMorita 28:fc6c46c1db65 104 scaledMotorOut[0]= dT;
NaotoMorita 31:8d02f3b9a067 105
NaotoMorita 36:e68ce293306e 106 double LP_servo = 0.2;
NaotoMorita 36:e68ce293306e 107 double LP_motor = 0.2;
NaotoMorita 36:e68ce293306e 108 for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
NaotoMorita 36:e68ce293306e 109 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
NaotoMorita 28:fc6c46c1db65 110 if(servoOut[i]<servoPwmMin) {
NaotoMorita 28:fc6c46c1db65 111 servoOut[i] = servoPwmMin;
NaotoMorita 28:fc6c46c1db65 112 };
NaotoMorita 28:fc6c46c1db65 113 if(servoOut[i]>servoPwmMax) {
NaotoMorita 28:fc6c46c1db65 114 servoOut[i] = servoPwmMax;
NaotoMorita 28:fc6c46c1db65 115 };
NaotoMorita 28:fc6c46c1db65 116 }
NaotoMorita 28:fc6c46c1db65 117
NaotoMorita 36:e68ce293306e 118 for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
NaotoMorita 36:e68ce293306e 119 motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
NaotoMorita 28:fc6c46c1db65 120 if(motorOut[i]<motorPwmMin) {
NaotoMorita 28:fc6c46c1db65 121 motorOut[i] = motorPwmMin;
NaotoMorita 28:fc6c46c1db65 122 };
NaotoMorita 28:fc6c46c1db65 123 if(motorOut[i]>motorPwmMax) {
NaotoMorita 28:fc6c46c1db65 124 motorOut[i] = motorPwmMax;
NaotoMorita 28:fc6c46c1db65 125 };
NaotoMorita 28:fc6c46c1db65 126 }
NaotoMorita 36:e68ce293306e 127 servoRight.pulsewidth_us(servoOut[0]);
NaotoMorita 36:e68ce293306e 128 servoLeft.pulsewidth_us(servoOut[1]);
NaotoMorita 36:e68ce293306e 129 servoThrust.pulsewidth_us(motorOut[0]);
NaotoMorita 32:c4f06cb0e1d6 130
NaotoMorita 36:e68ce293306e 131 if(loop_count > 10){
NaotoMorita 31:8d02f3b9a067 132 writeSdcard();
NaotoMorita 36:e68ce293306e 133 loop_count = 1;
NaotoMorita 27:43bd0e444633 134 }else{
NaotoMorita 27:43bd0e444633 135 loop_count +=1;
naker 26:62d9857eaecc 136 }
naker 22:da9893b71f24 137 }
cocorlow 7:70161eb0f854 138
NaotoMorita 27:43bd0e444633 139 void updateAttitude(){
NaotoMorita 17:6b7a363d06d2 140 // gx gy gz ax ay az
NaotoMorita 17:6b7a363d06d2 141 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 17:6b7a363d06d2 142 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 17:6b7a363d06d2 143 acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 17:6b7a363d06d2 144 acc_y = ay / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 145 acc_z = az / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 146 // 角速度値を分解能で割って角速度(deg per sec)に変換する
NaotoMorita 17:6b7a363d06d2 147 gyro_x = gx / GYRO_SSF; // (deg/s)
NaotoMorita 17:6b7a363d06d2 148 gyro_y = gy / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 149 gyro_z = gz / GYRO_SSF;
naker 20:2c3f113a8e8f 150 //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
NaotoMorita 36:e68ce293306e 151 //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
NaotoMorita 27:43bd0e444633 152 }
NaotoMorita 27:43bd0e444633 153
NaotoMorita 37:dad55cf05e3d 154 void getIMUval(){
NaotoMorita 36:e68ce293306e 155 // gx gy gz ax ay az
NaotoMorita 36:e68ce293306e 156 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 36:e68ce293306e 157 ax = ax - offsetVal[0];
NaotoMorita 36:e68ce293306e 158 ay = ay - offsetVal[1];
NaotoMorita 36:e68ce293306e 159 az = az - offsetVal[2];
NaotoMorita 36:e68ce293306e 160 gx = gx - offsetVal[3];
NaotoMorita 36:e68ce293306e 161 gy = gy - offsetVal[4];
NaotoMorita 36:e68ce293306e 162 gz = gz - offsetVal[5];
NaotoMorita 36:e68ce293306e 163 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 37:dad55cf05e3d 164 acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 37:dad55cf05e3d 165 acc_y = float(ay) / ACCEL_SSF;
NaotoMorita 37:dad55cf05e3d 166 acc_z = float(az) / ACCEL_SSF;
NaotoMorita 36:e68ce293306e 167 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 36:e68ce293306e 168 gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
NaotoMorita 36:e68ce293306e 169 gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
NaotoMorita 36:e68ce293306e 170 gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
NaotoMorita 37:dad55cf05e3d 171 }
NaotoMorita 37:dad55cf05e3d 172
NaotoMorita 37:dad55cf05e3d 173 void updateBetweenMeasures(){
NaotoMorita 36:e68ce293306e 174
NaotoMorita 37:dad55cf05e3d 175 getIMUval();
NaotoMorita 36:e68ce293306e 176
NaotoMorita 36:e68ce293306e 177 Matrix phi(4,4);
NaotoMorita 36:e68ce293306e 178 phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
NaotoMorita 36:e68ce293306e 179 << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
NaotoMorita 36:e68ce293306e 180 << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
NaotoMorita 36:e68ce293306e 181 << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
NaotoMorita 36:e68ce293306e 182 qhat = phi*qhat;
NaotoMorita 36:e68ce293306e 183
NaotoMorita 36:e68ce293306e 184 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 36:e68ce293306e 185 qhat *= (1.0f/ qnorm);
NaotoMorita 36:e68ce293306e 186
NaotoMorita 37:dad55cf05e3d 187 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 37:dad55cf05e3d 188 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 37:dad55cf05e3d 189 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 37:dad55cf05e3d 190 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 37:dad55cf05e3d 191
NaotoMorita 37:dad55cf05e3d 192 Matrix B(4,3);
NaotoMorita 37:dad55cf05e3d 193 B << q1 << q2 << q3
NaotoMorita 37:dad55cf05e3d 194 <<-q0 << q3 <<-q2
NaotoMorita 37:dad55cf05e3d 195 <<-q3 <<-q0 << q1
NaotoMorita 37:dad55cf05e3d 196 << q2 <<-q1 <<-q0;
NaotoMorita 37:dad55cf05e3d 197 B *= 0.5f;
NaotoMorita 37:dad55cf05e3d 198 Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
NaotoMorita 37:dad55cf05e3d 199
NaotoMorita 37:dad55cf05e3d 200
NaotoMorita 37:dad55cf05e3d 201 }
NaotoMorita 37:dad55cf05e3d 202
NaotoMorita 37:dad55cf05e3d 203 void updateAcrossMeasures(){
NaotoMorita 37:dad55cf05e3d 204 getIMUval();
NaotoMorita 37:dad55cf05e3d 205
NaotoMorita 37:dad55cf05e3d 206 Matrix uacc(3,1);
NaotoMorita 37:dad55cf05e3d 207 Matrix vacc(3,1);
NaotoMorita 37:dad55cf05e3d 208
NaotoMorita 37:dad55cf05e3d 209 uacc << 0 << 0 << -1;
NaotoMorita 37:dad55cf05e3d 210 float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
NaotoMorita 37:dad55cf05e3d 211 vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm;
NaotoMorita 37:dad55cf05e3d 212
NaotoMorita 37:dad55cf05e3d 213 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 37:dad55cf05e3d 214 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 37:dad55cf05e3d 215 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 37:dad55cf05e3d 216 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 37:dad55cf05e3d 217
NaotoMorita 37:dad55cf05e3d 218 Matrix A1(3,3);
NaotoMorita 37:dad55cf05e3d 219 A1 << q0 << q3 << -q2
NaotoMorita 37:dad55cf05e3d 220 <<-q3 << q0 << q1
NaotoMorita 37:dad55cf05e3d 221 <<q2 <<-q1 <<q0;
NaotoMorita 37:dad55cf05e3d 222 A1 *= 2.0f;
NaotoMorita 37:dad55cf05e3d 223
NaotoMorita 37:dad55cf05e3d 224 Matrix A2(3,3);
NaotoMorita 37:dad55cf05e3d 225 A2 << q1 << q2 << q3
NaotoMorita 37:dad55cf05e3d 226 << q2 <<-q1 << q0
NaotoMorita 37:dad55cf05e3d 227 << q3 <<-q0 <<-q1;
NaotoMorita 37:dad55cf05e3d 228 A2 *= 2.0f;
NaotoMorita 37:dad55cf05e3d 229
NaotoMorita 37:dad55cf05e3d 230 Matrix A3(3,3);
NaotoMorita 37:dad55cf05e3d 231 A3 <<-q2 << q1 <<-q0
NaotoMorita 37:dad55cf05e3d 232 << q1 << q2 << q3
NaotoMorita 37:dad55cf05e3d 233 << q0 << q3 <<-q2;
NaotoMorita 37:dad55cf05e3d 234 A3 *= 2.0f;
NaotoMorita 37:dad55cf05e3d 235
NaotoMorita 37:dad55cf05e3d 236 Matrix A4(3,3);
NaotoMorita 37:dad55cf05e3d 237 A4 <<-q3 << q0 << q1
NaotoMorita 37:dad55cf05e3d 238 <<-q0 <<-q3 << q2
NaotoMorita 37:dad55cf05e3d 239 << q1 << q2 << q3;
NaotoMorita 37:dad55cf05e3d 240 A4 *= 2.0f;
NaotoMorita 37:dad55cf05e3d 241
NaotoMorita 37:dad55cf05e3d 242 Matrix H(3,4);
NaotoMorita 37:dad55cf05e3d 243
NaotoMorita 37:dad55cf05e3d 244 Matrix ab1(A1*uacc);
NaotoMorita 37:dad55cf05e3d 245 Matrix ab2(A2*uacc);
NaotoMorita 37:dad55cf05e3d 246 Matrix ab3(A3*uacc);
NaotoMorita 37:dad55cf05e3d 247 Matrix ab4(A4*uacc);
NaotoMorita 37:dad55cf05e3d 248
NaotoMorita 37:dad55cf05e3d 249 H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
NaotoMorita 37:dad55cf05e3d 250 << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
NaotoMorita 37:dad55cf05e3d 251 << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
NaotoMorita 37:dad55cf05e3d 252
NaotoMorita 37:dad55cf05e3d 253 Matrix D(3,3);
NaotoMorita 37:dad55cf05e3d 254 D << q0*q0 + q1*q1 - q2*q2 - q3*q3
NaotoMorita 37:dad55cf05e3d 255 << 2*(q1*q2 + q0*q3)
NaotoMorita 37:dad55cf05e3d 256 << 2*(q1*q3 - q0*q2)
NaotoMorita 37:dad55cf05e3d 257 << 2*(q1*q2 - q0*q3)
NaotoMorita 37:dad55cf05e3d 258 << q0*q0 - q1*q1 + q2*q2 - q3*q3
NaotoMorita 37:dad55cf05e3d 259 << 2*(q2*q3 + q0*q1)
NaotoMorita 37:dad55cf05e3d 260 << 2*(q1*q3 + q0*q2)
NaotoMorita 37:dad55cf05e3d 261 << 2*(q2*q3 - q0*q1)
NaotoMorita 37:dad55cf05e3d 262 << q0*q0 - q1*q1 - q2*q2 + q3*q3;
NaotoMorita 37:dad55cf05e3d 263
NaotoMorita 37:dad55cf05e3d 264 Matrix err(3,1);
NaotoMorita 37:dad55cf05e3d 265 err = vacc-D*uacc;
NaotoMorita 37:dad55cf05e3d 266
NaotoMorita 37:dad55cf05e3d 267 Matrix K(4,3);
NaotoMorita 37:dad55cf05e3d 268 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc);
NaotoMorita 37:dad55cf05e3d 269
NaotoMorita 37:dad55cf05e3d 270 Matrix dq(4,1);
NaotoMorita 37:dad55cf05e3d 271 dq = K*err;
NaotoMorita 37:dad55cf05e3d 272 qhat = qhat-dq;
NaotoMorita 37:dad55cf05e3d 273
NaotoMorita 37:dad55cf05e3d 274 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 37:dad55cf05e3d 275 qhat *= (1.0f/ qnorm);
NaotoMorita 36:e68ce293306e 276 }
NaotoMorita 36:e68ce293306e 277
NaotoMorita 36:e68ce293306e 278 void computeAngles()
NaotoMorita 36:e68ce293306e 279 {
NaotoMorita 36:e68ce293306e 280 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 36:e68ce293306e 281 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 36:e68ce293306e 282 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 36:e68ce293306e 283 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 36:e68ce293306e 284 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
NaotoMorita 36:e68ce293306e 285 pitch = asinf(-2.0f * (q1*q3 - q0*q2));
NaotoMorita 36:e68ce293306e 286 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 33:e79457192a4b 287 }
NaotoMorita 33:e79457192a4b 288
NaotoMorita 27:43bd0e444633 289 int main()
NaotoMorita 36:e68ce293306e 290 {
NaotoMorita 28:fc6c46c1db65 291 pitchPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 292 pitchratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 293 rollPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 294 rollratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 295 pitchPID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 296 pitchratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 297 rollPID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 298 rollratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 299 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 300 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 301 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 29:34ee662f454e 302 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 303 pitchPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 304 pitchratePID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 305 rollPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 306 rollratePID.setInputLimits(-pi,pi);
NaotoMorita 29:34ee662f454e 307
NaotoMorita 36:e68ce293306e 308 servoRight.period_us(15000.0);
NaotoMorita 36:e68ce293306e 309 servoLeft.period_us(15000.0);
NaotoMorita 36:e68ce293306e 310 servoThrust.period_us(15000.0);
NaotoMorita 36:e68ce293306e 311 servoRight.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 312 servoLeft.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 313 servoThrust.pulsewidth_us(1100.0);
NaotoMorita 36:e68ce293306e 314
NaotoMorita 36:e68ce293306e 315 qhat << 1 << 0 << 0 << 0;
NaotoMorita 36:e68ce293306e 316
NaotoMorita 36:e68ce293306e 317 Phat << 0.0001 << 0 <<0 <<0
NaotoMorita 36:e68ce293306e 318 << 0 << 0.0001 <<0 <<0
NaotoMorita 36:e68ce293306e 319 << 0 << 0 <<0.0001 <<0
NaotoMorita 36:e68ce293306e 320 << 0 << 0 << 0 << 0.0001;
NaotoMorita 37:dad55cf05e3d 321
NaotoMorita 37:dad55cf05e3d 322 Qgyro << 0.0001 << 0 <<0
NaotoMorita 37:dad55cf05e3d 323 << 0 << 0.0001 <<0
NaotoMorita 37:dad55cf05e3d 324 << 0 << 0 <<0.0001;
NaotoMorita 37:dad55cf05e3d 325
NaotoMorita 36:e68ce293306e 326 Racc << 0.0001 << 0 <<0
NaotoMorita 36:e68ce293306e 327 << 0 << 0.0001 <<0
NaotoMorita 36:e68ce293306e 328 << 0 << 0 <<0.0001;
NaotoMorita 36:e68ce293306e 329
NaotoMorita 36:e68ce293306e 330
NaotoMorita 36:e68ce293306e 331 LoopTicker PIDtick;
NaotoMorita 36:e68ce293306e 332 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 37:dad55cf05e3d 333 pc.baud(115200);
NaotoMorita 36:e68ce293306e 334 //sd.baud(115200);
NaotoMorita 36:e68ce293306e 335 accelgyro.initialize();
NaotoMorita 36:e68ce293306e 336 //加速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 337 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 36:e68ce293306e 338 //■角速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 339 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 36:e68ce293306e 340 //MPU6050のLPFを設定
NaotoMorita 36:e68ce293306e 341 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 36:e68ce293306e 342 //MadgwickFilter.begin(100); //サンプリング周波数Hza
NaotoMorita 27:43bd0e444633 343 t.start();
NaotoMorita 36:e68ce293306e 344 //servoRight.write(0.0f);
NaotoMorita 27:43bd0e444633 345 while(1) {
NaotoMorita 31:8d02f3b9a067 346 float tstart = t.read();
NaotoMorita 27:43bd0e444633 347 //姿勢角を更新
NaotoMorita 36:e68ce293306e 348 //updateAttitude();
NaotoMorita 36:e68ce293306e 349 updateBetweenMeasures();
NaotoMorita 37:dad55cf05e3d 350 updateAcrossMeasures();
NaotoMorita 36:e68ce293306e 351 computeAngles();
NaotoMorita 28:fc6c46c1db65 352 PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
naker 22:da9893b71f24 353
NaotoMorita 17:6b7a363d06d2 354 float tend = t.read();
NaotoMorita 36:e68ce293306e 355 att_dt = (tend-tstart);
NaotoMorita 36:e68ce293306e 356 //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz
NaotoMorita 28:fc6c46c1db65 357
naker 20:2c3f113a8e8f 358 }
NaotoMorita 0:6b18a09a6628 359 }