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
Diff: main.cpp
- Revision:
- 17:6b7a363d06d2
- Parent:
- 16:802f457588c6
- Child:
- 19:61d7e9b4b6c2
--- a/main.cpp Thu Jan 28 05:27:17 2021 +0000 +++ b/main.cpp Thu Feb 04 09:16:08 2021 +0000 @@ -1,88 +1,145 @@ #include "mbed.h" #include "SBUS.hpp" -#include "IMUfilter.h" +#include <MadgwickAHRS.h> #include "LoopTicker.hpp" #include "MPU6050.h" +#include <I2Cdev.h> +//#include <Servo.h> +#define MPU6050_PWR_MGMT_1 0x6B +#define MPU_ADDRESS 0x68 +#define pi 3.141562 + +//加速度 Full-Scale Range +//#define ACCEL_FSR MPU6050_ACCEL_FS_2 +//#define ACCEL_FSR MPU6050_ACCEL_FS_4 +#define ACCEL_FSR MPU6050_ACCEL_FS_8 +//#define ACCEL_FSR MPU6050_ACCEL_FS_16 +//加速度 Sensitivity Scale Factor +#define ACCEL_SSF 36500.0 +//角速度 Full-Scale Range +#define GYRO_FSR MPU6050_GYRO_FS_250 +//#define GYRO_FSR MPU6050_GYRO_FS_500 +//#define GYRO_FSR MPU6050_GYRO_FS_1000 +//#define GYRO_FSR MPU6050_GYRO_FS_2000 +//角速度 Sensitivity Scale Factor +#define GYRO_SSF 131.0 +//#define GYRO_SSF 65.5 +//#define GYRO_SSF 32.8 +//#define GYRO_SSF 16.4 +#define MPU6050_LPF MPU6050_DLPF_BW_256 +//#define MPU6050_LPF MPU6050_DLPF_BW_188 +//#define MPU6050_LPF MPU6050_DLPF_BW_98 +//#define MPU6050_LPF MPU6050_DLPF_BW_42 +//#define MPU6050_LPF MPU6050_DLPF_BW_20 +//#define MPU6050_LPF MPU6050_DLPF_BW_10 +//#define MPU6050_LPF MPU6050_DLPF_BW_5 +MPU6050 accelgyro; +Madgwick MadgwickFilter; SBUS sbus(PD_5, PD_6); -MPU6050 mpu(PB_9,PB_8); Serial pc(USBTX, USBRX); -IMUfilter filter(0.01, 0.1); DigitalOut led1(LED1); DigitalOut led3(LED3); -PwmOut servo1(PB_4); -PwmOut servo2(PB_5); +PwmOut servoRight(PE_9); +//Servo servoRight(PB_4); //右のサーボ +//Servo servoLeft(PB_5); //左のサーボ +//Servo thrServo(PB_6); //スロットルサーボ + +Timer t; int ch1, ch2; float rc1, rc2; double pitch = 0.0; double roll = 0.0; double yaw = 0.0; -float gyro[3] = {0,0,0}; -float acc[3] = {0,0,1.0}; +int16_t ax, ay, az; +int16_t gx, gy, gz; +double acc_x,acc_y,acc_z; +double gyro_x,gyro_y,gyro_z; int out1, out2; +const double pitch_align = 0.0; +const double roll_align = -0.0; + long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } -void interrupt_SBUS() -{ - led1 = !led1; - if(sbus.failSafe == false) - { - ch1 = int(sbus.getData(1)); - ch2 = int(sbus.getData(2)); - pc.printf("ch1 :%d ", ch1); - pc.printf("ch2 :%d ", ch2); - float LP_rc = 0.65f; - float LP_rc3 = 0.15f; - rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; - rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; - pc.printf("rc1 :%f ", rc1); - pc.printf("rc2 :%f ", rc1); - int pwmMax = 1800; - int pwmMin = 1200; - out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); - if(out1<pwmMin){out1 = pwmMin;}; - if(out1>pwmMax){out1 = pwmMax;}; - out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); - if(out2<pwmMin){out2 = pwmMin;}; - if(out2>pwmMax){out2 = pwmMax;}; - } - filter.computeEuler(); - pitch = filter.getPitch(); - - pc.printf("pitch:%f ", pitch); - pc.printf("out1:%d ", out1); - pc.printf("out2:%d\r\n", out2); - servo1.pulsewidth_us(out1); - servo2.pulsewidth_us(out2); -} - -void interrupt_Gyro() -{ - // gx gy gz ax ay az - mpu.getGyro(gyro); - mpu.getAccelero(acc); - - 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]); -} - int main() { - mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); - mpu.setGyroRange(MPU6050_GYRO_RANGE_250); - pc.baud(9600); - servo1.period(0.020); - servo2.period(0.020); - LoopTicker timer_SBUS; - LoopTicker timer_Gyro; - timer_SBUS.attach(interrupt_SBUS, 0.02); - timer_Gyro.attach(interrupt_Gyro, 0.01); + pc.baud(115200); + accelgyro.initialize(); + uint8_t offsetVal[6] = {0,0,0,0,0,0}; + accelgyro.setXAccelOffset(offsetVal[0]); + accelgyro.setYAccelOffset(offsetVal[1]); + accelgyro.setZAccelOffset(offsetVal[2]); + accelgyro.setXGyroOffset(offsetVal[3]); + accelgyro.setYGyroOffset(offsetVal[4]); + accelgyro.setZGyroOffset(offsetVal[5]); + //加速度計のフルスケールレンジを設定 + accelgyro.setFullScaleAccelRange(ACCEL_FSR); + //■角速度計のフルスケールレンジを設定 + accelgyro.setFullScaleGyroRange(GYRO_FSR); + //MPU6050のLPFを設定 + accelgyro.setDLPFMode(MPU6050_LPF); + + servoRight.period_us(20000); + + MadgwickFilter.begin(100); //サンプリング周波数Hza + t.start(); while(1) { - timer_SBUS.loop(); - timer_Gyro.loop(); - } + float tstart = t.read(); + if(sbus.failSafe == false) + { + ch1 = int(sbus.getData(1)); + ch2 = int(sbus.getData(2)); + //pc.printf("ch1 :%d ", ch1); + //pc.printf("ch2 :%d ", ch2); + float LP_rc = 0.65f; + float LP_rc3 = 0.15f; + rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; + rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; + //pc.printf("rc1 :%f ", rc1); + //pc.printf("rc2 :%f ", rc1); + int pwmMax = 1800; + int pwmMin = 1200; + out1 = map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); + if(out1<pwmMin){out1 = pwmMin;}; + if(out1>pwmMax){out1 = pwmMax;}; + out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); + if(out2<pwmMin){out2 = pwmMin;}; + if(out2>pwmMax){out2 = pwmMax;}; + } + + // gx gy gz ax ay az + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + // 加速度値を分解能で割って加速度(G)に変換する + acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g + acc_y = ay / ACCEL_SSF; + acc_z = az / ACCEL_SSF; + // 角速度値を分解能で割って角速度(deg per sec)に変換する + gyro_x = gx / GYRO_SSF; // (deg/s) + gyro_y = gy / GYRO_SSF; + gyro_z = gz / GYRO_SSF; + //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 + //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z); + MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); + + pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; + roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; + + servoRight.pulsewidth_us(out1); + + pc.printf("out1:%d ", out1); + //pc.printf("roll%f ", roll*180.0/pi); + //pc.printf("out1:%d ", out1); + //pc.printf("out2:%d ", out2); + + float tend = t.read(); + wait(0.005); + MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza + pc.printf("time%f \r\n", (tend-tstart)); + + } } \ No newline at end of file