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@19:61d7e9b4b6c2, 2021-02-05 (annotated)
- Committer:
- naker
- Date:
- Fri Feb 05 04:58:07 2021 +0000
- Revision:
- 19:61d7e9b4b6c2
- Parent:
- 17:6b7a363d06d2
- Child:
- 20:2c3f113a8e8f
sd log test
Who changed what in which revision?
User | Revision | Line number | New 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); |
naker | 19:61d7e9b4b6c2 | 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); |
NaotoMorita | 17:6b7a363d06d2 | 45 | //Servo servoRight(PB_4); //右のサーボ |
NaotoMorita | 17:6b7a363d06d2 | 46 | //Servo servoLeft(PB_5); //左のサーボ |
NaotoMorita | 17:6b7a363d06d2 | 47 | //Servo thrServo(PB_6); //スロットルサーボ |
NaotoMorita | 17:6b7a363d06d2 | 48 | |
NaotoMorita | 17:6b7a363d06d2 | 49 | Timer t; |
cocorlow | 3:79e62f9b13c8 | 50 | |
NaotoMorita | 5:9cad4ce807b9 | 51 | int ch1, ch2; |
cocorlow | 3:79e62f9b13c8 | 52 | float rc1, rc2; |
naker | 15:6abaac6e5b03 | 53 | double pitch = 0.0; |
naker | 15:6abaac6e5b03 | 54 | double roll = 0.0; |
naker | 15:6abaac6e5b03 | 55 | double yaw = 0.0; |
NaotoMorita | 17:6b7a363d06d2 | 56 | int16_t ax, ay, az; |
NaotoMorita | 17:6b7a363d06d2 | 57 | int16_t gx, gy, gz; |
NaotoMorita | 17:6b7a363d06d2 | 58 | double acc_x,acc_y,acc_z; |
NaotoMorita | 17:6b7a363d06d2 | 59 | double gyro_x,gyro_y,gyro_z; |
cocorlow | 3:79e62f9b13c8 | 60 | int out1, out2; |
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 | 17:6b7a363d06d2 | 64 | |
cocorlow | 7:70161eb0f854 | 65 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cocorlow | 7:70161eb0f854 | 66 | { |
cocorlow | 7:70161eb0f854 | 67 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 7:70161eb0f854 | 68 | } |
cocorlow | 7:70161eb0f854 | 69 | |
NaotoMorita | 0:6b18a09a6628 | 70 | int main() |
NaotoMorita | 0:6b18a09a6628 | 71 | { |
naker | 19:61d7e9b4b6c2 | 72 | int count = 0; |
NaotoMorita | 17:6b7a363d06d2 | 73 | pc.baud(115200); |
naker | 19:61d7e9b4b6c2 | 74 | sd.baud(9600); |
NaotoMorita | 17:6b7a363d06d2 | 75 | accelgyro.initialize(); |
NaotoMorita | 17:6b7a363d06d2 | 76 | uint8_t offsetVal[6] = {0,0,0,0,0,0}; |
NaotoMorita | 17:6b7a363d06d2 | 77 | accelgyro.setXAccelOffset(offsetVal[0]); |
NaotoMorita | 17:6b7a363d06d2 | 78 | accelgyro.setYAccelOffset(offsetVal[1]); |
NaotoMorita | 17:6b7a363d06d2 | 79 | accelgyro.setZAccelOffset(offsetVal[2]); |
NaotoMorita | 17:6b7a363d06d2 | 80 | accelgyro.setXGyroOffset(offsetVal[3]); |
NaotoMorita | 17:6b7a363d06d2 | 81 | accelgyro.setYGyroOffset(offsetVal[4]); |
NaotoMorita | 17:6b7a363d06d2 | 82 | accelgyro.setZGyroOffset(offsetVal[5]); |
NaotoMorita | 17:6b7a363d06d2 | 83 | //加速度計のフルスケールレンジを設定 |
NaotoMorita | 17:6b7a363d06d2 | 84 | accelgyro.setFullScaleAccelRange(ACCEL_FSR); |
NaotoMorita | 17:6b7a363d06d2 | 85 | //■角速度計のフルスケールレンジを設定 |
NaotoMorita | 17:6b7a363d06d2 | 86 | accelgyro.setFullScaleGyroRange(GYRO_FSR); |
NaotoMorita | 17:6b7a363d06d2 | 87 | //MPU6050のLPFを設定 |
NaotoMorita | 17:6b7a363d06d2 | 88 | accelgyro.setDLPFMode(MPU6050_LPF); |
NaotoMorita | 17:6b7a363d06d2 | 89 | |
NaotoMorita | 17:6b7a363d06d2 | 90 | servoRight.period_us(20000); |
NaotoMorita | 17:6b7a363d06d2 | 91 | |
NaotoMorita | 17:6b7a363d06d2 | 92 | MadgwickFilter.begin(100); //サンプリング周波数Hza |
NaotoMorita | 17:6b7a363d06d2 | 93 | t.start(); |
NaotoMorita | 0:6b18a09a6628 | 94 | while(1) { |
naker | 19:61d7e9b4b6c2 | 95 | if(count == 50){ |
naker | 19:61d7e9b4b6c2 | 96 | sd.printf("test"); |
naker | 19:61d7e9b4b6c2 | 97 | count = 0; |
naker | 19:61d7e9b4b6c2 | 98 | } |
naker | 19:61d7e9b4b6c2 | 99 | count ++; |
NaotoMorita | 17:6b7a363d06d2 | 100 | float tstart = t.read(); |
NaotoMorita | 17:6b7a363d06d2 | 101 | if(sbus.failSafe == false) |
NaotoMorita | 17:6b7a363d06d2 | 102 | { |
NaotoMorita | 17:6b7a363d06d2 | 103 | ch1 = int(sbus.getData(1)); |
NaotoMorita | 17:6b7a363d06d2 | 104 | ch2 = int(sbus.getData(2)); |
NaotoMorita | 17:6b7a363d06d2 | 105 | //pc.printf("ch1 :%d ", ch1); |
NaotoMorita | 17:6b7a363d06d2 | 106 | //pc.printf("ch2 :%d ", ch2); |
NaotoMorita | 17:6b7a363d06d2 | 107 | float LP_rc = 0.65f; |
NaotoMorita | 17:6b7a363d06d2 | 108 | float LP_rc3 = 0.15f; |
NaotoMorita | 17:6b7a363d06d2 | 109 | rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; |
NaotoMorita | 17:6b7a363d06d2 | 110 | rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; |
NaotoMorita | 17:6b7a363d06d2 | 111 | //pc.printf("rc1 :%f ", rc1); |
NaotoMorita | 17:6b7a363d06d2 | 112 | //pc.printf("rc2 :%f ", rc1); |
NaotoMorita | 17:6b7a363d06d2 | 113 | int pwmMax = 1800; |
NaotoMorita | 17:6b7a363d06d2 | 114 | int pwmMin = 1200; |
NaotoMorita | 17:6b7a363d06d2 | 115 | out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); |
NaotoMorita | 17:6b7a363d06d2 | 116 | if(out1<pwmMin){out1 = pwmMin;}; |
NaotoMorita | 17:6b7a363d06d2 | 117 | if(out1>pwmMax){out1 = pwmMax;}; |
NaotoMorita | 17:6b7a363d06d2 | 118 | out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); |
NaotoMorita | 17:6b7a363d06d2 | 119 | if(out2<pwmMin){out2 = pwmMin;}; |
NaotoMorita | 17:6b7a363d06d2 | 120 | if(out2>pwmMax){out2 = pwmMax;}; |
NaotoMorita | 17:6b7a363d06d2 | 121 | } |
NaotoMorita | 17:6b7a363d06d2 | 122 | |
NaotoMorita | 17:6b7a363d06d2 | 123 | // gx gy gz ax ay az |
NaotoMorita | 17:6b7a363d06d2 | 124 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 17:6b7a363d06d2 | 125 | // 加速度値を分解能で割って加速度(G)に変換する |
NaotoMorita | 17:6b7a363d06d2 | 126 | acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g |
NaotoMorita | 17:6b7a363d06d2 | 127 | acc_y = ay / ACCEL_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 128 | acc_z = az / ACCEL_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 129 | // 角速度値を分解能で割って角速度(deg per sec)に変換する |
NaotoMorita | 17:6b7a363d06d2 | 130 | gyro_x = gx / GYRO_SSF; // (deg/s) |
NaotoMorita | 17:6b7a363d06d2 | 131 | gyro_y = gy / GYRO_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 132 | gyro_z = gz / GYRO_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 133 | //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 |
NaotoMorita | 17:6b7a363d06d2 | 134 | //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z); |
NaotoMorita | 17:6b7a363d06d2 | 135 | MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); |
NaotoMorita | 17:6b7a363d06d2 | 136 | |
NaotoMorita | 17:6b7a363d06d2 | 137 | pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; |
NaotoMorita | 17:6b7a363d06d2 | 138 | roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; |
NaotoMorita | 17:6b7a363d06d2 | 139 | |
NaotoMorita | 17:6b7a363d06d2 | 140 | servoRight.pulsewidth_us(out1); |
NaotoMorita | 17:6b7a363d06d2 | 141 | |
NaotoMorita | 17:6b7a363d06d2 | 142 | pc.printf("out1:%d ", out1); |
NaotoMorita | 17:6b7a363d06d2 | 143 | //pc.printf("roll%f ", roll*180.0/pi); |
NaotoMorita | 17:6b7a363d06d2 | 144 | //pc.printf("out1:%d ", out1); |
NaotoMorita | 17:6b7a363d06d2 | 145 | //pc.printf("out2:%d ", out2); |
NaotoMorita | 17:6b7a363d06d2 | 146 | |
NaotoMorita | 17:6b7a363d06d2 | 147 | float tend = t.read(); |
NaotoMorita | 17:6b7a363d06d2 | 148 | wait(0.005); |
NaotoMorita | 17:6b7a363d06d2 | 149 | MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza |
NaotoMorita | 17:6b7a363d06d2 | 150 | pc.printf("time%f \r\n", (tend-tstart)); |
NaotoMorita | 17:6b7a363d06d2 | 151 | |
NaotoMorita | 17:6b7a363d06d2 | 152 | } |
NaotoMorita | 0:6b18a09a6628 | 153 | } |