HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

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

main.cpp@33:e79457192a4b, 2021-03-02 (annotated)

Committer:
NaotoMorita
Date:
Tue Mar 02 04:28:08 2021 +0000
Revision:
33:e79457192a4b
Parent:
32:c4f06cb0e1d6
Child:
35:4535af88f7bf
test

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 17:6b7a363d06d2 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 33:e79457192a4b 8 #include "Matrix.h"
cocorlow 6:2cba569272fe 9
NaotoMorita 17:6b7a363d06d2 10 #define MPU6050_PWR_MGMT_1 0x6B
NaotoMorita 17:6b7a363d06d2 11 #define MPU_ADDRESS 0x68
NaotoMorita 17:6b7a363d06d2 12 #define pi 3.141562
NaotoMorita 17:6b7a363d06d2 13 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 17:6b7a363d06d2 14 #define ACCEL_SSF 36500.0
NaotoMorita 17:6b7a363d06d2 15 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 16 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 17 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 27:43bd0e444633 18
NaotoMorita 17:6b7a363d06d2 19 MPU6050 accelgyro;
NaotoMorita 17:6b7a363d06d2 20 Madgwick MadgwickFilter;
NaotoMorita 0:6b18a09a6628 21 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 22 Serial pc(USBTX, USBRX);
naker 26:62d9857eaecc 23 Serial sd(PG_14,PG_9);
cocorlow 3:79e62f9b13c8 24 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 25 DigitalOut led3(LED3);
NaotoMorita 27:43bd0e444633 26
NaotoMorita 17:6b7a363d06d2 27 PwmOut servoRight(PE_9);
NaotoMorita 32:c4f06cb0e1d6 28 PwmOut servoLeft(PE_11);
NaotoMorita 32:c4f06cb0e1d6 29 PwmOut servoThrust(PA_0);
NaotoMorita 32:c4f06cb0e1d6 30 const double PID_dt = 1.0/200;
NaotoMorita 31:8d02f3b9a067 31 PID pitchPID(3.0, 0,0,PID_dt); //rad
NaotoMorita 31:8d02f3b9a067 32 PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 31:8d02f3b9a067 33 PID rollPID(3.0,0.0,0.0,PID_dt);
NaotoMorita 31:8d02f3b9a067 34 PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 33:e79457192a4b 35 Quaternion qhat(1,0,0,0);
NaotoMorita 33:e79457192a4b 36 Matrix Phat(4,4);
NaotoMorita 33:e79457192a4b 37 Matrix Qgyro(3,3);
NaotoMorita 33:e79457192a4b 38 Matrix Racc(3,3);
NaotoMorita 17:6b7a363d06d2 39 Timer t;
cocorlow 3:79e62f9b13c8 40
NaotoMorita 27:43bd0e444633 41 int loop_count = 0;
naker 23:4a490fa8bf4a 42 float rc[16];
naker 25:5aca9b224226 43 float roll_ac;
naker 15:6abaac6e5b03 44 double pitch = 0.0;
naker 15:6abaac6e5b03 45 double roll = 0.0;
naker 15:6abaac6e5b03 46 double yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 47 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 48 int16_t gx, gy, gz;
NaotoMorita 17:6b7a363d06d2 49 double acc_x,acc_y,acc_z;
NaotoMorita 17:6b7a363d06d2 50 double gyro_x,gyro_y,gyro_z;
cocorlow 3:79e62f9b13c8 51 int out1, out2;
NaotoMorita 28:fc6c46c1db65 52 float scaledServoOut[3]= {0,0,0};
NaotoMorita 28:fc6c46c1db65 53 float scaledMotorOut[1]= {-1};
NaotoMorita 28:fc6c46c1db65 54 int servoOut[3] = {1500,1500,1500};
NaotoMorita 28:fc6c46c1db65 55 int motorOut[1] = {1000};
NaotoMorita 28:fc6c46c1db65 56 int servoPwmMax = 1800;
NaotoMorita 28:fc6c46c1db65 57 int servoPwmMin = 1200;
NaotoMorita 30:214ddc613a35 58 int motorPwmMax = 2000;
NaotoMorita 32:c4f06cb0e1d6 59 int motorPwmMin = 950;
NaotoMorita 27:43bd0e444633 60 int offsetVal[6] = {0,0,0,0,0,0};
cocorlow 3:79e62f9b13c8 61
NaotoMorita 17:6b7a363d06d2 62 const double pitch_align = 0.0;
NaotoMorita 17:6b7a363d06d2 63 const double roll_align = -0.0;
NaotoMorita 27:43bd0e444633 64
NaotoMorita 27:43bd0e444633 65
cocorlow 7:70161eb0f854 66 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 67 {
cocorlow 7:70161eb0f854 68 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 69 }
NaotoMorita 27:43bd0e444633 70
NaotoMorita 30:214ddc613a35 71 void writeSdcard()
naker 20:2c3f113a8e8f 72 {
NaotoMorita 32:c4f06cb0e1d6 73 pc.printf("motor: %d, servo1: %d, servo2: %d \r\n",motorOut[0],servoOut[0],servoOut[1]);
naker 20:2c3f113a8e8f 74 }
NaotoMorita 27:43bd0e444633 75
naker 22:da9893b71f24 76 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 77 void calcServoOut(){
NaotoMorita 28:fc6c46c1db65 78 if(sbus.failSafe == false) {
NaotoMorita 28:fc6c46c1db65 79 // sbusデータの読み込み
NaotoMorita 28:fc6c46c1db65 80 for (int i =0 ; i < 16;i ++){
NaotoMorita 28:fc6c46c1db65 81 rc[i] = 0.65f * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - 0.65f) * rc[i]; // mapped input
NaotoMorita 27:43bd0e444633 82 }
NaotoMorita 28:fc6c46c1db65 83 }
NaotoMorita 28:fc6c46c1db65 84
NaotoMorita 28:fc6c46c1db65 85 pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
NaotoMorita 28:fc6c46c1db65 86 roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
NaotoMorita 28:fc6c46c1db65 87 pitchPID.setProcessValue(pitch);
NaotoMorita 28:fc6c46c1db65 88 pitchratePID.setProcessValue(gyro_y);
NaotoMorita 28:fc6c46c1db65 89 rollPID.setProcessValue(roll);
NaotoMorita 28:fc6c46c1db65 90 rollratePID.setProcessValue(gyro_x);
NaotoMorita 28:fc6c46c1db65 91 float de = pitchPID.compute()+pitchratePID.compute()+rc[1];
NaotoMorita 28:fc6c46c1db65 92 float da = rollPID.compute()+rollratePID.compute()+rc[0];
NaotoMorita 28:fc6c46c1db65 93 float dT = rc[2];
NaotoMorita 28:fc6c46c1db65 94
NaotoMorita 28:fc6c46c1db65 95 scaledServoOut[0]=de+da;
NaotoMorita 28:fc6c46c1db65 96 scaledServoOut[1]=de-da;
NaotoMorita 28:fc6c46c1db65 97 scaledMotorOut[0]= dT;
NaotoMorita 31:8d02f3b9a067 98
NaotoMorita 31:8d02f3b9a067 99 for(int i = 0;i<4;i++){
NaotoMorita 31:8d02f3b9a067 100 servoOut[i] = int(map(scaledServoOut[i]*1000.0,-1000,1000,servoPwmMin,servoPwmMax));
NaotoMorita 28:fc6c46c1db65 101 if(servoOut[i]<servoPwmMin) {
NaotoMorita 28:fc6c46c1db65 102 servoOut[i] = servoPwmMin;
NaotoMorita 28:fc6c46c1db65 103 };
NaotoMorita 28:fc6c46c1db65 104 if(servoOut[i]>servoPwmMax) {
NaotoMorita 28:fc6c46c1db65 105 servoOut[i] = servoPwmMax;
NaotoMorita 28:fc6c46c1db65 106 };
NaotoMorita 28:fc6c46c1db65 107 }
NaotoMorita 28:fc6c46c1db65 108
NaotoMorita 31:8d02f3b9a067 109 for(int i = 0;i<1;i++){
NaotoMorita 31:8d02f3b9a067 110 motorOut[i] = int(map(scaledMotorOut[i]*1000.0,-1000,1000,motorPwmMin,motorPwmMax));
NaotoMorita 28:fc6c46c1db65 111 if(motorOut[i]<motorPwmMin) {
NaotoMorita 28:fc6c46c1db65 112 motorOut[i] = motorPwmMin;
NaotoMorita 28:fc6c46c1db65 113 };
NaotoMorita 28:fc6c46c1db65 114 if(motorOut[i]>motorPwmMax) {
NaotoMorita 28:fc6c46c1db65 115 motorOut[i] = motorPwmMax;
NaotoMorita 28:fc6c46c1db65 116 };
NaotoMorita 28:fc6c46c1db65 117 }
NaotoMorita 32:c4f06cb0e1d6 118
NaotoMorita 28:fc6c46c1db65 119 servoRight.pulsewidth_us(servoOut[0]);
NaotoMorita 32:c4f06cb0e1d6 120 servoLeft.pulsewidth_us(servoOut[1]);
NaotoMorita 32:c4f06cb0e1d6 121 servoThrust.pulsewidth_us(motorOut[0]);
NaotoMorita 32:c4f06cb0e1d6 122
NaotoMorita 31:8d02f3b9a067 123 if(loop_count > int(1/PID_dt/10.0)){
NaotoMorita 31:8d02f3b9a067 124 writeSdcard();
naker 26:62d9857eaecc 125 loop_count = 0;
NaotoMorita 27:43bd0e444633 126 }else{
NaotoMorita 27:43bd0e444633 127 loop_count +=1;
naker 26:62d9857eaecc 128 }
naker 22:da9893b71f24 129 }
cocorlow 7:70161eb0f854 130
NaotoMorita 27:43bd0e444633 131 void updateAttitude(){
NaotoMorita 17:6b7a363d06d2 132 // gx gy gz ax ay az
NaotoMorita 17:6b7a363d06d2 133 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 17:6b7a363d06d2 134 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 17:6b7a363d06d2 135 acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 17:6b7a363d06d2 136 acc_y = ay / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 137 acc_z = az / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 138 // 角速度値を分解能で割って角速度(deg per sec)に変換する
NaotoMorita 17:6b7a363d06d2 139 gyro_x = gx / GYRO_SSF; // (deg/s)
NaotoMorita 17:6b7a363d06d2 140 gyro_y = gy / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 141 gyro_z = gz / GYRO_SSF;
naker 20:2c3f113a8e8f 142 //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
NaotoMorita 17:6b7a363d06d2 143 MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
NaotoMorita 27:43bd0e444633 144 }
NaotoMorita 27:43bd0e444633 145
NaotoMorita 33:e79457192a4b 146 void updateBetweenMeasures(){
NaotoMorita 33:e79457192a4b 147
NaotoMorita 33:e79457192a4b 148 }
NaotoMorita 33:e79457192a4b 149
NaotoMorita 27:43bd0e444633 150 int main()
NaotoMorita 27:43bd0e444633 151 {
NaotoMorita 27:43bd0e444633 152 LoopTicker PIDtick;
NaotoMorita 28:fc6c46c1db65 153 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 32:c4f06cb0e1d6 154 //pc.baud(115200);
NaotoMorita 32:c4f06cb0e1d6 155 //sd.baud(115200);
NaotoMorita 27:43bd0e444633 156 accelgyro.initialize();
NaotoMorita 27:43bd0e444633 157 //加速度計のフルスケールレンジを設定
NaotoMorita 27:43bd0e444633 158 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 27:43bd0e444633 159 //■角速度計のフルスケールレンジを設定
NaotoMorita 27:43bd0e444633 160 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 27:43bd0e444633 161 //MPU6050のLPFを設定
NaotoMorita 27:43bd0e444633 162 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 28:fc6c46c1db65 163
NaotoMorita 28:fc6c46c1db65 164 pitchPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 165 pitchratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 166 rollPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 167 rollratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 168 pitchPID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 169 pitchratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 170 rollPID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 171 rollratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 172 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 173 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 174 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 29:34ee662f454e 175 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 176 pitchPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 177 pitchratePID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 178 rollPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 179 rollratePID.setInputLimits(-pi,pi);
NaotoMorita 29:34ee662f454e 180
NaotoMorita 27:43bd0e444633 181 servoRight.period_us(20000);
NaotoMorita 32:c4f06cb0e1d6 182 servoLeft.period_us(20000);
NaotoMorita 32:c4f06cb0e1d6 183 servoThrust.period_us(20000);
NaotoMorita 27:43bd0e444633 184 MadgwickFilter.begin(100); //サンプリング周波数Hza
NaotoMorita 27:43bd0e444633 185 t.start();
NaotoMorita 27:43bd0e444633 186 while(1) {
NaotoMorita 31:8d02f3b9a067 187 float tstart = t.read();
NaotoMorita 27:43bd0e444633 188 //姿勢角を更新
NaotoMorita 29:34ee662f454e 189 updateAttitude();
NaotoMorita 28:fc6c46c1db65 190 PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
naker 22:da9893b71f24 191
NaotoMorita 17:6b7a363d06d2 192 float tend = t.read();
NaotoMorita 28:fc6c46c1db65 193 MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz
NaotoMorita 28:fc6c46c1db65 194
naker 20:2c3f113a8e8f 195 }
NaotoMorita 0:6b18a09a6628 196 }