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@24:6ab61527855d, 2021-02-15 (annotated)

Committer:
naker
Date:
Mon Feb 15 07:14:44 2021 +0000
Revision:
24:6ab61527855d
Parent:
23:4a490fa8bf4a
Child:
25:5aca9b224226
no comment

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
NaotoMorita 17:6b7a363d06d2 13 //加速度 Full-Scale Range
NaotoMorita 17:6b7a363d06d2 14 //#define ACCEL_FSR MPU6050_ACCEL_FS_2
NaotoMorita 17:6b7a363d06d2 15 //#define ACCEL_FSR MPU6050_ACCEL_FS_4
NaotoMorita 17:6b7a363d06d2 16 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 17:6b7a363d06d2 17 //#define ACCEL_FSR MPU6050_ACCEL_FS_16
NaotoMorita 17:6b7a363d06d2 18 //加速度 Sensitivity Scale Factor
NaotoMorita 17:6b7a363d06d2 19 #define ACCEL_SSF 36500.0
NaotoMorita 17:6b7a363d06d2 20 //角速度 Full-Scale Range
NaotoMorita 17:6b7a363d06d2 21 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 22 //#define GYRO_FSR MPU6050_GYRO_FS_500
NaotoMorita 17:6b7a363d06d2 23 //#define GYRO_FSR MPU6050_GYRO_FS_1000
NaotoMorita 17:6b7a363d06d2 24 //#define GYRO_FSR MPU6050_GYRO_FS_2000
NaotoMorita 17:6b7a363d06d2 25 //角速度 Sensitivity Scale Factor
NaotoMorita 17:6b7a363d06d2 26 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 27 //#define GYRO_SSF 65.5
NaotoMorita 17:6b7a363d06d2 28 //#define GYRO_SSF 32.8
NaotoMorita 17:6b7a363d06d2 29 //#define GYRO_SSF 16.4
NaotoMorita 17:6b7a363d06d2 30 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 17:6b7a363d06d2 31 //#define MPU6050_LPF MPU6050_DLPF_BW_188
naker 20:2c3f113a8e8f 32 //#define MPU6050_LPF MPU6050_DLPF_BW_98
naker 20:2c3f113a8e8f 33 //#define MPU6050_LPF MPU6050_DLPF_BW_42
naker 20:2c3f113a8e8f 34 //#define MPU6050_LPF MPU6050_DLPF_BW_20
naker 20:2c3f113a8e8f 35 //#define MPU6050_LPF MPU6050_DLPF_BW_10
naker 20:2c3f113a8e8f 36 //#define MPU6050_LPF MPU6050_DLPF_BW_5
NaotoMorita 17:6b7a363d06d2 37 MPU6050 accelgyro;
NaotoMorita 17:6b7a363d06d2 38 Madgwick MadgwickFilter;
NaotoMorita 0:6b18a09a6628 39 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 40 Serial pc(USBTX, USBRX);
naker 20:2c3f113a8e8f 41 // Serial sd(PE_8,PE_7);
cocorlow 3:79e62f9b13c8 42 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 43 DigitalOut led3(LED3);
NaotoMorita 17:6b7a363d06d2 44 PwmOut servoRight(PE_9);
naker 20:2c3f113a8e8f 45 //PwmOut co(PE_9); // サーボのoutput
naker 20:2c3f113a8e8f 46 //Servo servoRight(PE_); //右のサーボ
NaotoMorita 17:6b7a363d06d2 47 //Servo servoLeft(PB_5); //左のサーボ
NaotoMorita 17:6b7a363d06d2 48 //Servo thrServo(PB_6); //スロットルサーボ
NaotoMorita 17:6b7a363d06d2 49
naker 20:2c3f113a8e8f 50 // のセットアップ
naker 20:2c3f113a8e8f 51 // (float Kp, float Ki, float Kd, float tSample);
naker 20:2c3f113a8e8f 52 const double PID_dt = 0.01;
naker 21:df4e4e857a3e 53 PID test_control(1.2, 0.0, 0.0, PID_dt);
naker 22:da9893b71f24 54 PID pitchPID(1.0, 0,0,PID_dt); //rad
naker 22:da9893b71f24 55 PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
naker 22:da9893b71f24 56 PID rollPID(1.0,0.0,0.0,PID_dt);
naker 22:da9893b71f24 57 PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 58 Timer t;
cocorlow 3:79e62f9b13c8 59
NaotoMorita 5:9cad4ce807b9 60 int ch1, ch2;
naker 23:4a490fa8bf4a 61 float rc[16];
naker 23:4a490fa8bf4a 62 //float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ
naker 15:6abaac6e5b03 63 double pitch = 0.0;
naker 15:6abaac6e5b03 64 double roll = 0.0;
naker 15:6abaac6e5b03 65 double yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 66 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 67 int16_t gx, gy, gz;
NaotoMorita 17:6b7a363d06d2 68 double acc_x,acc_y,acc_z;
NaotoMorita 17:6b7a363d06d2 69 double gyro_x,gyro_y,gyro_z;
cocorlow 3:79e62f9b13c8 70 int out1, out2;
naker 24:6ab61527855d 71 int scaler_servo[3];
naker 24:6ab61527855d 72 int servo_out[3];
cocorlow 3:79e62f9b13c8 73
NaotoMorita 17:6b7a363d06d2 74 const double pitch_align = 0.0;
NaotoMorita 17:6b7a363d06d2 75 const double roll_align = -0.0;
naker 20:2c3f113a8e8f 76 float tstart;
cocorlow 7:70161eb0f854 77 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 78 {
cocorlow 7:70161eb0f854 79 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 80 }
naker 20:2c3f113a8e8f 81 void pushto_sdcard()
naker 20:2c3f113a8e8f 82 {
naker 20:2c3f113a8e8f 83 // sd.printf("tstart: %f \n",tstart);
naker 20:2c3f113a8e8f 84 }
naker 22:da9893b71f24 85 // 割り込まれた時点での出力(computeの結果）を返す関数
naker 22:da9893b71f24 86 /*
naker 22:da9893b71f24 87 float PID_compute(){
naker 22:da9893b71f24 88
naker 22:da9893b71f24 89 }
naker 22:da9893b71f24 90 */
cocorlow 7:70161eb0f854 91
NaotoMorita 0:6b18a09a6628 92 int main()
NaotoMorita 0:6b18a09a6628 93 {
naker 20:2c3f113a8e8f 94 //LoopTicker sdcard_loop;
naker 22:da9893b71f24 95 LoopTicker PIDtick;
naker 24:6ab61527855d 96 //PIDtick.attach(test_control,PID_dt);
naker 20:2c3f113a8e8f 97 //sdcard_loop.attach(pushto_sdcard,0.01);
NaotoMorita 17:6b7a363d06d2 98 pc.baud(115200);
naker 20:2c3f113a8e8f 99 //sd.baud(115200);
NaotoMorita 17:6b7a363d06d2 100 accelgyro.initialize();
NaotoMorita 17:6b7a363d06d2 101 uint8_t offsetVal[6] = {0,0,0,0,0,0};
NaotoMorita 17:6b7a363d06d2 102 accelgyro.setXAccelOffset(offsetVal[0]);
NaotoMorita 17:6b7a363d06d2 103 accelgyro.setYAccelOffset(offsetVal[1]);
NaotoMorita 17:6b7a363d06d2 104 accelgyro.setZAccelOffset(offsetVal[2]);
NaotoMorita 17:6b7a363d06d2 105 accelgyro.setXGyroOffset(offsetVal[3]);
NaotoMorita 17:6b7a363d06d2 106 accelgyro.setYGyroOffset(offsetVal[4]);
NaotoMorita 17:6b7a363d06d2 107 accelgyro.setZGyroOffset(offsetVal[5]);
NaotoMorita 17:6b7a363d06d2 108 //加速度計のフルスケールレンジを設定
NaotoMorita 17:6b7a363d06d2 109 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 17:6b7a363d06d2 110 //■角速度計のフルスケールレンジを設定
NaotoMorita 17:6b7a363d06d2 111 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 17:6b7a363d06d2 112 //MPU6050のLPFを設定
NaotoMorita 17:6b7a363d06d2 113 accelgyro.setDLPFMode(MPU6050_LPF);
naker 20:2c3f113a8e8f 114
NaotoMorita 17:6b7a363d06d2 115 servoRight.period_us(20000);
naker 20:2c3f113a8e8f 116
NaotoMorita 17:6b7a363d06d2 117 MadgwickFilter.begin(100); //サンプリング周波数Hza
naker 20:2c3f113a8e8f 118 // のセットアップ
naker 20:2c3f113a8e8f 119 test_control.setInputLimits(-60.0,60.0); // 60°が限界
naker 24:6ab61527855d 120 test_control.setOutputLimits(1200, 1800);
naker 24:6ab61527855d 121 test_control.setBias((1200+1800)/2); // out range の真ん中に設定する（out rangeの中央が安定点のため）
NaotoMorita 17:6b7a363d06d2 122 t.start();
naker 22:da9893b71f24 123 float LP_rc = 0.65f;
NaotoMorita 0:6b18a09a6628 124 while(1) {
naker 20:2c3f113a8e8f 125 tstart = t.read();
naker 20:2c3f113a8e8f 126 //sdcard_loop.loop();
naker 20:2c3f113a8e8f 127 if(sbus.failSafe == false) {
naker 22:da9893b71f24 128 // sbusデータの読み込み
naker 22:da9893b71f24 129 for (int i =0 ; i < 16;i ++){
naker 22:da9893b71f24 130 int data;
naker 22:da9893b71f24 131 float mapped_data;
naker 23:4a490fa8bf4a 132 rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input
naker 22:da9893b71f24 133 }
naker 23:4a490fa8bf4a 134 pc.printf("data: %d",rc[1]);
NaotoMorita 17:6b7a363d06d2 135 }
naker 20:2c3f113a8e8f 136
NaotoMorita 17:6b7a363d06d2 137 // gx gy gz ax ay az
NaotoMorita 17:6b7a363d06d2 138 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 17:6b7a363d06d2 139 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 17:6b7a363d06d2 140 acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 17:6b7a363d06d2 141 acc_y = ay / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 142 acc_z = az / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 143 // 角速度値を分解能で割って角速度(deg per sec)に変換する
NaotoMorita 17:6b7a363d06d2 144 gyro_x = gx / GYRO_SSF; // (deg/s)
NaotoMorita 17:6b7a363d06d2 145 gyro_y = gy / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 146 gyro_z = gz / GYRO_SSF;
naker 20:2c3f113a8e8f 147 //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
NaotoMorita 17:6b7a363d06d2 148 //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z);
NaotoMorita 17:6b7a363d06d2 149 MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
NaotoMorita 17:6b7a363d06d2 150
NaotoMorita 17:6b7a363d06d2 151 pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
NaotoMorita 17:6b7a363d06d2 152 roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
naker 22:da9893b71f24 153
naker 20:2c3f113a8e8f 154 //float LP_rc3 = 0.15f;
naker 24:6ab61527855d 155 roll_ac = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac;
naker 22:da9893b71f24 156 //rc1 = LP_rc * float(map(ch1,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc1; // mapped input
naker 22:da9893b71f24 157
naker 22:da9893b71f24 158 //rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
naker 20:2c3f113a8e8f 159 int pwmMax = 1800;
naker 20:2c3f113a8e8f 160 int pwmMin = 1200;
naker 24:6ab61527855d 161 out1 = map((int)（de _left*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
naker 20:2c3f113a8e8f 162 if(out1<pwmMin) {
naker 20:2c3f113a8e8f 163 out1 = pwmMin;
naker 20:2c3f113a8e8f 164 };
naker 20:2c3f113a8e8f 165 if(out1>pwmMax) {
naker 20:2c3f113a8e8f 166 out1 = pwmMax;
naker 20:2c3f113a8e8f 167 };
naker 23:4a490fa8bf4a 168 out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
naker 20:2c3f113a8e8f 169 if(out2<pwmMin) {
naker 20:2c3f113a8e8f 170 out2 = pwmMin;
naker 20:2c3f113a8e8f 171 };
naker 20:2c3f113a8e8f 172 if(out2>pwmMax) {
naker 20:2c3f113a8e8f 173 out2 = pwmMax;
naker 20:2c3f113a8e8f 174 };
naker 23:4a490fa8bf4a 175 //test_control.setSetPoint(rc[1]);
naker 22:da9893b71f24 176 test_control.setSetPoint(0.0); // 仮（実際にはsbusの読み込みなど）
naker 20:2c3f113a8e8f 177 pc.printf("roll%f ", roll*180.0/pi);
naker 22:da9893b71f24 178 //pc.printf("out1:%d\n ", out1);
NaotoMorita 17:6b7a363d06d2 179 //pc.printf("out2:%d ", out2);
naker 20:2c3f113a8e8f 180 wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節（割り込みにするべき）
NaotoMorita 17:6b7a363d06d2 181 float tend = t.read();
NaotoMorita 17:6b7a363d06d2 182 MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
NaotoMorita 17:6b7a363d06d2 183 pc.printf("time%f \r\n", (tend-tstart));
naker 20:2c3f113a8e8f 184 test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう
naker 24:6ab61527855d 185 scaled_servo[0] = test_control.compute(); // -1 ~ 1
naker 24:6ab61527855d 186 servo_out[0] = map((int)(scaler_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用
naker 24:6ab61527855d 187 for (int i = 0; i < 3 ; i ++){
naker 24:6ab61527855d 188 servo_out[i] = max(servo_out[i],pwemMin);
naker 24:6ab61527855d 189 servo_out[i] = min(servo_out[i], pwmMax);
naker 24:6ab61527855d 190 }
naker 24:6ab61527855d 191 /*
naker 24:6ab61527855d 192 if(out1<pwmMin) {
naker 24:6ab61527855d 193 out1 = pwmMin;
naker 24:6ab61527855d 194 };
naker 24:6ab61527855d 195 if(out1>pwmMax) {
naker 24:6ab61527855d 196 out1 = pwmMax;
naker 24:6ab61527855d 197 };
naker 24:6ab61527855d 198 out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax);
naker 24:6ab61527855d 199 if(out2<pwmMin) {
naker 24:6ab61527855d 200 out2 = pwmMin;
naker 24:6ab61527855d 201 };
naker 24:6ab61527855d 202 if(out2>pwmMax) {
naker 24:6ab61527855d 203 out2 = pwmMax;
naker 24:6ab61527855d 204 };
naker 24:6ab61527855d 205 */
naker 24:6ab61527855d 206 pc.printf("out1:%d ",servo_out[i]);
naker 24:6ab61527855d 207 servoRight.pulsewidth_us(servo_out[i]);
naker 20:2c3f113a8e8f 208 }
NaotoMorita 0:6b18a09a6628 209 }