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@16:802f457588c6, 2021-01-28 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Jan 28 05:27:17 2021 +0000
- Revision:
- 16:802f457588c6
- Parent:
- 15:6abaac6e5b03
- Child:
- 17:6b7a363d06d2
compute euler
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 | 13:ac7c1c861aaf | 3 | #include "IMUfilter.h" |
cocorlow | 6:2cba569272fe | 4 | #include "LoopTicker.hpp" |
naker | 15:6abaac6e5b03 | 5 | #include "MPU6050.h" |
cocorlow | 6:2cba569272fe | 6 | |
NaotoMorita | 0:6b18a09a6628 | 7 | SBUS sbus(PD_5, PD_6); |
naker | 15:6abaac6e5b03 | 8 | MPU6050 mpu(PB_9,PB_8); |
NaotoMorita | 0:6b18a09a6628 | 9 | Serial pc(USBTX, USBRX); |
NaotoMorita | 16:802f457588c6 | 10 | IMUfilter filter(0.01, 0.1); |
cocorlow | 3:79e62f9b13c8 | 11 | DigitalOut led1(LED1); |
cocorlow | 3:79e62f9b13c8 | 12 | DigitalOut led3(LED3); |
cocorlow | 3:79e62f9b13c8 | 13 | PwmOut servo1(PB_4); |
cocorlow | 3:79e62f9b13c8 | 14 | PwmOut servo2(PB_5); |
cocorlow | 3:79e62f9b13c8 | 15 | |
NaotoMorita | 5:9cad4ce807b9 | 16 | int ch1, ch2; |
cocorlow | 3:79e62f9b13c8 | 17 | float rc1, rc2; |
naker | 15:6abaac6e5b03 | 18 | double pitch = 0.0; |
naker | 15:6abaac6e5b03 | 19 | double roll = 0.0; |
naker | 15:6abaac6e5b03 | 20 | double yaw = 0.0; |
naker | 14:468ccb88e387 | 21 | float gyro[3] = {0,0,0}; |
naker | 14:468ccb88e387 | 22 | float acc[3] = {0,0,1.0}; |
cocorlow | 3:79e62f9b13c8 | 23 | int out1, out2; |
cocorlow | 3:79e62f9b13c8 | 24 | |
cocorlow | 7:70161eb0f854 | 25 | long map(long x, long in_min, long in_max, long out_min, long out_max) |
cocorlow | 7:70161eb0f854 | 26 | { |
cocorlow | 7:70161eb0f854 | 27 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 7:70161eb0f854 | 28 | } |
cocorlow | 7:70161eb0f854 | 29 | |
cocorlow | 3:79e62f9b13c8 | 30 | void interrupt_SBUS() |
cocorlow | 3:79e62f9b13c8 | 31 | { |
cocorlow | 3:79e62f9b13c8 | 32 | led1 = !led1; |
cocorlow | 3:79e62f9b13c8 | 33 | if(sbus.failSafe == false) |
cocorlow | 3:79e62f9b13c8 | 34 | { |
cocorlow | 3:79e62f9b13c8 | 35 | ch1 = int(sbus.getData(1)); |
cocorlow | 3:79e62f9b13c8 | 36 | ch2 = int(sbus.getData(2)); |
cocorlow | 6:2cba569272fe | 37 | pc.printf("ch1 :%d ", ch1); |
cocorlow | 6:2cba569272fe | 38 | pc.printf("ch2 :%d ", ch2); |
cocorlow | 6:2cba569272fe | 39 | float LP_rc = 0.65f; |
cocorlow | 6:2cba569272fe | 40 | float LP_rc3 = 0.15f; |
cocorlow | 7:70161eb0f854 | 41 | rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; |
cocorlow | 7:70161eb0f854 | 42 | rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; |
cocorlow | 6:2cba569272fe | 43 | pc.printf("rc1 :%f ", rc1); |
cocorlow | 6:2cba569272fe | 44 | pc.printf("rc2 :%f ", rc1); |
cocorlow | 3:79e62f9b13c8 | 45 | int pwmMax = 1800; |
cocorlow | 3:79e62f9b13c8 | 46 | int pwmMin = 1200; |
cocorlow | 7:70161eb0f854 | 47 | out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); |
cocorlow | 3:79e62f9b13c8 | 48 | if(out1<pwmMin){out1 = pwmMin;}; |
cocorlow | 3:79e62f9b13c8 | 49 | if(out1>pwmMax){out1 = pwmMax;}; |
cocorlow | 7:70161eb0f854 | 50 | out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); |
cocorlow | 3:79e62f9b13c8 | 51 | if(out2<pwmMin){out2 = pwmMin;}; |
cocorlow | 3:79e62f9b13c8 | 52 | if(out2>pwmMax){out2 = pwmMax;}; |
cocorlow | 3:79e62f9b13c8 | 53 | } |
NaotoMorita | 16:802f457588c6 | 54 | filter.computeEuler(); |
NaotoMorita | 10:d2d196cf93ab | 55 | pitch = filter.getPitch(); |
NaotoMorita | 10:d2d196cf93ab | 56 | |
naker | 15:6abaac6e5b03 | 57 | pc.printf("pitch:%f ", pitch); |
NaotoMorita | 10:d2d196cf93ab | 58 | pc.printf("out1:%d ", out1); |
NaotoMorita | 10:d2d196cf93ab | 59 | pc.printf("out2:%d\r\n", out2); |
NaotoMorita | 10:d2d196cf93ab | 60 | servo1.pulsewidth_us(out1); |
NaotoMorita | 10:d2d196cf93ab | 61 | servo2.pulsewidth_us(out2); |
cocorlow | 3:79e62f9b13c8 | 62 | } |
cocorlow | 3:79e62f9b13c8 | 63 | |
cocorlow | 3:79e62f9b13c8 | 64 | void interrupt_Gyro() |
cocorlow | 3:79e62f9b13c8 | 65 | { |
naker | 14:468ccb88e387 | 66 | // gx gy gz ax ay az |
naker | 15:6abaac6e5b03 | 67 | mpu.getGyro(gyro); |
naker | 14:468ccb88e387 | 68 | mpu.getAccelero(acc); |
NaotoMorita | 16:802f457588c6 | 69 | |
NaotoMorita | 16:802f457588c6 | 70 | filter.updateFilter(gyro[0]/180.0*3.14,gyro[1]/180.0*3.14,gyro[2]/180.0*3.14,acc[0],acc[1],acc[2]); |
cocorlow | 3:79e62f9b13c8 | 71 | } |
cocorlow | 3:79e62f9b13c8 | 72 | |
NaotoMorita | 0:6b18a09a6628 | 73 | int main() |
NaotoMorita | 0:6b18a09a6628 | 74 | { |
naker | 14:468ccb88e387 | 75 | mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); |
naker | 15:6abaac6e5b03 | 76 | mpu.setGyroRange(MPU6050_GYRO_RANGE_250); |
NaotoMorita | 0:6b18a09a6628 | 77 | pc.baud(9600); |
cocorlow | 3:79e62f9b13c8 | 78 | servo1.period(0.020); |
cocorlow | 3:79e62f9b13c8 | 79 | servo2.period(0.020); |
cocorlow | 6:2cba569272fe | 80 | LoopTicker timer_SBUS; |
cocorlow | 6:2cba569272fe | 81 | LoopTicker timer_Gyro; |
NaotoMorita | 16:802f457588c6 | 82 | timer_SBUS.attach(interrupt_SBUS, 0.02); |
NaotoMorita | 16:802f457588c6 | 83 | timer_Gyro.attach(interrupt_Gyro, 0.01); |
NaotoMorita | 0:6b18a09a6628 | 84 | while(1) { |
cocorlow | 6:2cba569272fe | 85 | timer_SBUS.loop(); |
NaotoMorita | 10:d2d196cf93ab | 86 | timer_Gyro.loop(); |
NaotoMorita | 0:6b18a09a6628 | 87 | } |
NaotoMorita | 0:6b18a09a6628 | 88 | } |