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@17:6b7a363d06d2, 2021-02-04 (annotated)

Committer:
NaotoMorita
Date:
Thu Feb 04 09:16:08 2021 +0000
Revision:
17:6b7a363d06d2
Parent:
16:802f457588c6
Child:
19:61d7e9b4b6c2
servo ahrs gyro OK

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 0:6b18a09a6628 1 #include "mbed.h"
NaotoMorita 0:6b18a09a6628 2 #include "SBUS.hpp"
NaotoMorita 17:6b7a363d06d2 3 #include <MadgwickAHRS.h>
cocorlow 6:2cba569272fe 4 #include "LoopTicker.hpp"
naker 15:6abaac6e5b03 5 #include "MPU6050.h"
NaotoMorita 17:6b7a363d06d2 6 #include <I2Cdev.h>
NaotoMorita 17:6b7a363d06d2 7 //#include <Servo.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
NaotoMorita 17:6b7a363d06d2 32 //#define MPU6050_LPF MPU6050_DLPF_BW_98
NaotoMorita 17:6b7a363d06d2 33 //#define MPU6050_LPF MPU6050_DLPF_BW_42
NaotoMorita 17:6b7a363d06d2 34 //#define MPU6050_LPF MPU6050_DLPF_BW_20
NaotoMorita 17:6b7a363d06d2 35 //#define MPU6050_LPF MPU6050_DLPF_BW_10
NaotoMorita 17:6b7a363d06d2 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);
cocorlow 3:79e62f9b13c8 41 DigitalOut led1(LED1);
cocorlow 3:79e62f9b13c8 42 DigitalOut led3(LED3);
NaotoMorita 17:6b7a363d06d2 43 PwmOut servoRight(PE_9);
NaotoMorita 17:6b7a363d06d2 44 //Servo servoRight(PB_4); //右のサーボ
NaotoMorita 17:6b7a363d06d2 45 //Servo servoLeft(PB_5); //左のサーボ
NaotoMorita 17:6b7a363d06d2 46 //Servo thrServo(PB_6); //スロットルサーボ
NaotoMorita 17:6b7a363d06d2 47
NaotoMorita 17:6b7a363d06d2 48 Timer t;
cocorlow 3:79e62f9b13c8 49
NaotoMorita 5:9cad4ce807b9 50 int ch1, ch2;
cocorlow 3:79e62f9b13c8 51 float rc1, rc2;
naker 15:6abaac6e5b03 52 double pitch = 0.0;
naker 15:6abaac6e5b03 53 double roll = 0.0;
naker 15:6abaac6e5b03 54 double yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 55 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 56 int16_t gx, gy, gz;
NaotoMorita 17:6b7a363d06d2 57 double acc_x,acc_y,acc_z;
NaotoMorita 17:6b7a363d06d2 58 double gyro_x,gyro_y,gyro_z;
cocorlow 3:79e62f9b13c8 59 int out1, out2;
cocorlow 3:79e62f9b13c8 60
NaotoMorita 17:6b7a363d06d2 61 const double pitch_align = 0.0;
NaotoMorita 17:6b7a363d06d2 62 const double roll_align = -0.0;
NaotoMorita 17:6b7a363d06d2 63
cocorlow 7:70161eb0f854 64 long map(long x, long in_min, long in_max, long out_min, long out_max)
cocorlow 7:70161eb0f854 65 {
cocorlow 7:70161eb0f854 66 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 67 }
cocorlow 7:70161eb0f854 68
NaotoMorita 0:6b18a09a6628 69 int main()
NaotoMorita 0:6b18a09a6628 70 {
NaotoMorita 17:6b7a363d06d2 71 pc.baud(115200);
NaotoMorita 17:6b7a363d06d2 72 accelgyro.initialize();
NaotoMorita 17:6b7a363d06d2 73 uint8_t offsetVal[6] = {0,0,0,0,0,0};
NaotoMorita 17:6b7a363d06d2 74 accelgyro.setXAccelOffset(offsetVal[0]);
NaotoMorita 17:6b7a363d06d2 75 accelgyro.setYAccelOffset(offsetVal[1]);
NaotoMorita 17:6b7a363d06d2 76 accelgyro.setZAccelOffset(offsetVal[2]);
NaotoMorita 17:6b7a363d06d2 77 accelgyro.setXGyroOffset(offsetVal[3]);
NaotoMorita 17:6b7a363d06d2 78 accelgyro.setYGyroOffset(offsetVal[4]);
NaotoMorita 17:6b7a363d06d2 79 accelgyro.setZGyroOffset(offsetVal[5]);
NaotoMorita 17:6b7a363d06d2 80 //加速度計のフルスケールレンジを設定
NaotoMorita 17:6b7a363d06d2 81 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 17:6b7a363d06d2 82 //■角速度計のフルスケールレンジを設定
NaotoMorita 17:6b7a363d06d2 83 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 17:6b7a363d06d2 84 //MPU6050のLPFを設定
NaotoMorita 17:6b7a363d06d2 85 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 17:6b7a363d06d2 86
NaotoMorita 17:6b7a363d06d2 87 servoRight.period_us(20000);
NaotoMorita 17:6b7a363d06d2 88
NaotoMorita 17:6b7a363d06d2 89 MadgwickFilter.begin(100); //サンプリング周波数Hza
NaotoMorita 17:6b7a363d06d2 90 t.start();
NaotoMorita 0:6b18a09a6628 91 while(1) {
NaotoMorita 17:6b7a363d06d2 92 float tstart = t.read();
NaotoMorita 17:6b7a363d06d2 93 if(sbus.failSafe == false)
NaotoMorita 17:6b7a363d06d2 94 {
NaotoMorita 17:6b7a363d06d2 95 ch1 = int(sbus.getData(1));
NaotoMorita 17:6b7a363d06d2 96 ch2 = int(sbus.getData(2));
NaotoMorita 17:6b7a363d06d2 97 //pc.printf("ch1 :%d ", ch1);
NaotoMorita 17:6b7a363d06d2 98 //pc.printf("ch2 :%d ", ch2);
NaotoMorita 17:6b7a363d06d2 99 float LP_rc = 0.65f;
NaotoMorita 17:6b7a363d06d2 100 float LP_rc3 = 0.15f;
NaotoMorita 17:6b7a363d06d2 101 rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
NaotoMorita 17:6b7a363d06d2 102 rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
NaotoMorita 17:6b7a363d06d2 103 //pc.printf("rc1 :%f ", rc1);
NaotoMorita 17:6b7a363d06d2 104 //pc.printf("rc2 :%f ", rc1);
NaotoMorita 17:6b7a363d06d2 105 int pwmMax = 1800;
NaotoMorita 17:6b7a363d06d2 106 int pwmMin = 1200;
NaotoMorita 17:6b7a363d06d2 107 out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
NaotoMorita 17:6b7a363d06d2 108 if(out1<pwmMin){out1 = pwmMin;};
NaotoMorita 17:6b7a363d06d2 109 if(out1>pwmMax){out1 = pwmMax;};
NaotoMorita 17:6b7a363d06d2 110 out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
NaotoMorita 17:6b7a363d06d2 111 if(out2<pwmMin){out2 = pwmMin;};
NaotoMorita 17:6b7a363d06d2 112 if(out2>pwmMax){out2 = pwmMax;};
NaotoMorita 17:6b7a363d06d2 113 }
NaotoMorita 17:6b7a363d06d2 114
NaotoMorita 17:6b7a363d06d2 115 // gx gy gz ax ay az
NaotoMorita 17:6b7a363d06d2 116 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
NaotoMorita 17:6b7a363d06d2 117 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 17:6b7a363d06d2 118 acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 17:6b7a363d06d2 119 acc_y = ay / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 120 acc_z = az / ACCEL_SSF;
NaotoMorita 17:6b7a363d06d2 121 // 角速度値を分解能で割って角速度(deg per sec)に変換する
NaotoMorita 17:6b7a363d06d2 122 gyro_x = gx / GYRO_SSF; // (deg/s)
NaotoMorita 17:6b7a363d06d2 123 gyro_y = gy / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 124 gyro_z = gz / GYRO_SSF;
NaotoMorita 17:6b7a363d06d2 125 //Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
NaotoMorita 17:6b7a363d06d2 126 //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z);
NaotoMorita 17:6b7a363d06d2 127 MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
NaotoMorita 17:6b7a363d06d2 128
NaotoMorita 17:6b7a363d06d2 129 pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180;
NaotoMorita 17:6b7a363d06d2 130 roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180;
NaotoMorita 17:6b7a363d06d2 131
NaotoMorita 17:6b7a363d06d2 132 servoRight.pulsewidth_us(out1);
NaotoMorita 17:6b7a363d06d2 133
NaotoMorita 17:6b7a363d06d2 134 pc.printf("out1:%d ", out1);
NaotoMorita 17:6b7a363d06d2 135 //pc.printf("roll%f ", roll*180.0/pi);
NaotoMorita 17:6b7a363d06d2 136 //pc.printf("out1:%d ", out1);
NaotoMorita 17:6b7a363d06d2 137 //pc.printf("out2:%d ", out2);
NaotoMorita 17:6b7a363d06d2 138
NaotoMorita 17:6b7a363d06d2 139 float tend = t.read();
NaotoMorita 17:6b7a363d06d2 140 wait(0.005);
NaotoMorita 17:6b7a363d06d2 141 MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza
NaotoMorita 17:6b7a363d06d2 142 pc.printf("time%f \r\n", (tend-tstart));
NaotoMorita 17:6b7a363d06d2 143
NaotoMorita 17:6b7a363d06d2 144 }
NaotoMorita 0:6b18a09a6628 145 }