HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

main.cpp@14:468ccb88e387, 2021-01-28 (annotated)

Committer:
naker
Date:
Thu Jan 28 04:41:40 2021 +0000
Revision:
14:468ccb88e387
Parent:
13:ac7c1c861aaf
Child:
15:6abaac6e5b03
add 'read gyro' function

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