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@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?

UserRevisionLine numberNew 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 }