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