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@27:43bd0e444633, 2021-02-16 (annotated)

Committer:
NaotoMorita
Date:
Tue Feb 16 02:56:16 2021 +0000
Revision:
27:43bd0e444633
Parent:
26:62d9857eaecc
Child:
28:fc6c46c1db65
modify code

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>
cocorlow 6:2cba569272fe 8
NaotoMorita 17:6b7a363d06d2 9 #define MPU6050_PWR_MGMT_1 0x6B
NaotoMorita 17:6b7a363d06d2 10 #define MPU_ADDRESS 0x68
NaotoMorita 17:6b7a363d06d2 11 #define pi 3.141562
NaotoMorita 17:6b7a363d06d2 12 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 17:6b7a363d06d2 13 #define ACCEL_SSF 36500.0
NaotoMorita 17:6b7a363d06d2 14 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 15 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 16 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 27:43bd0e444633 17
NaotoMorita 17:6b7a363d06d2 18 MPU6050 accelgyro;
NaotoMorita 17:6b7a363d06d2 19 Madgwick MadgwickFilter;
NaotoMorita 0:6b18a09a6628 20 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 21 Serial pc(USBTX, USBRX);
naker 26:62d9857eaecc 22 Serial sd(PG_14,PG_9);
cocorlow 3:79e62f9b13c8 23 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 24 DigitalOut led3(LED3);
NaotoMorita 27:43bd0e444633 25
NaotoMorita 17:6b7a363d06d2 26 PwmOut servoRight(PE_9);
naker 20:2c3f113a8e8f 27 const double PID_dt = 0.01;
naker 22:da9893b71f24 28 PID pitchPID(1.0, 0,0,PID_dt); //rad
naker 22:da9893b71f24 29 PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
naker 22:da9893b71f24 30 PID rollPID(1.0,0.0,0.0,PID_dt);
naker 22:da9893b71f24 31 PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 32 Timer t;
cocorlow 3:79e62f9b13c8 33
NaotoMorita 27:43bd0e444633 34 int loop_count = 0;
naker 23:4a490fa8bf4a 35 float rc[16];
naker 25:5aca9b224226 36 float roll_ac;
naker 15:6abaac6e5b03 37 double pitch = 0.0;
naker 15:6abaac6e5b03 38 double roll = 0.0;
naker 15:6abaac6e5b03 39 double yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 40 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 41 int16_t gx, gy, gz;
NaotoMorita 17:6b7a363d06d2 42 double acc_x,acc_y,acc_z;
NaotoMorita 17:6b7a363d06d2 43 double gyro_x,gyro_y,gyro_z;
cocorlow 3:79e62f9b13c8 44 int out1, out2;
naker 25:5aca9b224226 45 float scaled_servo[3];
NaotoMorita 27:43bd0e444633 46 int servo_out[3] = {0,0,0};
NaotoMorita 27:43bd0e444633 47 int motor_out[1] = {0};
NaotoMorita 27:43bd0e444633 48 int offsetVal[6] = {0,0,0,0,0,0};
cocorlow 3:79e62f9b13c8 49
NaotoMorita 17:6b7a363d06d2 50 const double pitch_align = 0.0;
NaotoMorita 17:6b7a363d06d2 51 const double roll_align = -0.0;
naker 20:2c3f113a8e8f 52 float tstart;
NaotoMorita 27:43bd0e444633 53
NaotoMorita 27:43bd0e444633 54
cocorlow 7:70161eb0f854 55 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 56 {
cocorlow 7:70161eb0f854 57 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 58 }
NaotoMorita 27:43bd0e444633 59
naker 20:2c3f113a8e8f 60 void pushto_sdcard()
naker 20:2c3f113a8e8f 61 {
naker 26:62d9857eaecc 62 sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll);
naker 20:2c3f113a8e8f 63 }
NaotoMorita 27:43bd0e444633 64
naker 22:da9893b71f24 65 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 66 void calcServoOut(){
NaotoMorita 27:43bd0e444633 67 if(sbus.failSafe == false) {
NaotoMorita 27:43bd0e444633 68 // sbusデータの読み込み
NaotoMorita 27:43bd0e444633 69 for (int i =0 ; i < 16;i ++){
NaotoMorita 27:43bd0e444633 70 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 71 }
NaotoMorita 27:43bd0e444633 72 }
NaotoMorita 27:43bd0e444633 73 if(loop_count > 10)
naker 26:62d9857eaecc 74 pushto_sdcard();
naker 26:62d9857eaecc 75 loop_count = 0;
NaotoMorita 27:43bd0e444633 76 }else{
NaotoMorita 27:43bd0e444633 77 loop_count +=1;
naker 26:62d9857eaecc 78 }
naker 22:da9893b71f24 79 }
cocorlow 7:70161eb0f854 80
NaotoMorita 27:43bd0e444633 81 void updateAttitude(){
NaotoMorita 17:6b7a363d06d2 82 // gx gy gz ax ay az
NaotoMorita 17:6b7a363d06d2 83 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 17:6b7a363d06d2 84 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 17:6b7a363d06d2 85 acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 17:6b7a363d06d2 86 acc_y = ay / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 87 acc_z = az / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 88 // 角速度値を分解能で割って角速度(deg per sec)に変換する
NaotoMorita 17:6b7a363d06d2 89 gyro_x = gx / GYRO_SSF; // (deg/s)
NaotoMorita 17:6b7a363d06d2 90 gyro_y = gy / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 91 gyro_z = gz / GYRO_SSF;
naker 20:2c3f113a8e8f 92 //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
NaotoMorita 17:6b7a363d06d2 93 MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
NaotoMorita 27:43bd0e444633 94 }
NaotoMorita 27:43bd0e444633 95
NaotoMorita 27:43bd0e444633 96 int main()
NaotoMorita 27:43bd0e444633 97 {
NaotoMorita 27:43bd0e444633 98 LoopTicker sdcard_loop;
NaotoMorita 27:43bd0e444633 99 LoopTicker PIDtick;
NaotoMorita 27:43bd0e444633 100 PIDtick.attach(PID_compute,PID_dt);
NaotoMorita 27:43bd0e444633 101 pc.baud(115200);
NaotoMorita 27:43bd0e444633 102 sd.baud(115200);
NaotoMorita 27:43bd0e444633 103 accelgyro.initialize();
NaotoMorita 27:43bd0e444633 104 //加速度計のフルスケールレンジを設定
NaotoMorita 27:43bd0e444633 105 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 27:43bd0e444633 106 //■角速度計のフルスケールレンジを設定
NaotoMorita 27:43bd0e444633 107 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 27:43bd0e444633 108 //MPU6050のLPFを設定
NaotoMorita 27:43bd0e444633 109 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 27:43bd0e444633 110
NaotoMorita 27:43bd0e444633 111 servoRight.period_us(20000);
NaotoMorita 27:43bd0e444633 112
NaotoMorita 27:43bd0e444633 113 MadgwickFilter.begin(100); //サンプリング周波数Hza
NaotoMorita 27:43bd0e444633 114 t.start();
NaotoMorita 27:43bd0e444633 115 while(1) {
NaotoMorita 27:43bd0e444633 116 tstart = t.read();
NaotoMorita 27:43bd0e444633 117 //姿勢角を更新
NaotoMorita 27:43bd0e444633 118 updateAttitude()
NaotoMorita 17:6b7a363d06d2 119
NaotoMorita 17:6b7a363d06d2 120 pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
NaotoMorita 17:6b7a363d06d2 121 roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
naker 22:da9893b71f24 122
naker 20:2c3f113a8e8f 123 //float LP_rc3 = 0.15f;
NaotoMorita 27:43bd0e444633 124 servoOut = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac;
naker 20:2c3f113a8e8f 125 int pwmMax = 1800;
naker 20:2c3f113a8e8f 126 int pwmMin = 1200;
naker 20:2c3f113a8e8f 127 if(out1<pwmMin) {
naker 20:2c3f113a8e8f 128 out1 = pwmMin;
naker 20:2c3f113a8e8f 129 };
naker 20:2c3f113a8e8f 130 if(out1>pwmMax) {
naker 20:2c3f113a8e8f 131 out1 = pwmMax;
naker 20:2c3f113a8e8f 132 };
naker 23:4a490fa8bf4a 133 out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
naker 20:2c3f113a8e8f 134 if(out2<pwmMin) {
naker 20:2c3f113a8e8f 135 out2 = pwmMin;
naker 20:2c3f113a8e8f 136 };
naker 20:2c3f113a8e8f 137 if(out2>pwmMax) {
naker 20:2c3f113a8e8f 138 out2 = pwmMax;
naker 20:2c3f113a8e8f 139 };
naker 26:62d9857eaecc 140 pc.printf("roll%lf ", gyro_x);
naker 22:da9893b71f24 141 //pc.printf("out1:%d\n ", out1);
NaotoMorita 17:6b7a363d06d2 142 //pc.printf("out2:%d ", out2);
naker 20:2c3f113a8e8f 143 wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
NaotoMorita 17:6b7a363d06d2 144 float tend = t.read();
NaotoMorita 17:6b7a363d06d2 145 MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
NaotoMorita 17:6b7a363d06d2 146 pc.printf("time%f \r\n", (tend-tstart));
naker 20:2c3f113a8e8f 147 test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
naker 25:5aca9b224226 148 PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
naker 26:62d9857eaecc 149 //sdcard_loop.loop();
naker 25:5aca9b224226 150 printf("scaled_servo[0]: %f",scaled_servo[0]);
naker 25:5aca9b224226 151 servo_out[0] = map((int)(scaled_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
naker 24:6ab61527855d 152 for (int i = 0; i < 3 ; i ++){
naker 25:5aca9b224226 153 servo_out[i] = max(servo_out[i],pwmMin);
naker 24:6ab61527855d 154 servo_out[i] = min(servo_out[i], pwmMax);
naker 24:6ab61527855d 155 }
naker 25:5aca9b224226 156 pc.printf("out1:%d ",servo_out[0]);
naker 25:5aca9b224226 157 servoRight.pulsewidth_us(servo_out[0]);
naker 20:2c3f113a8e8f 158 }
NaotoMorita 0:6b18a09a6628 159 }