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
- Committer:
- naker
- Date:
- 2021-02-05
- Revision:
- 19:61d7e9b4b6c2
- Parent:
- 17:6b7a363d06d2
- Child:
- 20:2c3f113a8e8f
File content as of revision 19:61d7e9b4b6c2:
#include "mbed.h" #include "SBUS.hpp" #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); Serial pc(USBTX, USBRX); Serial sd(PE_8,PE_7); DigitalOut led1(LED1); DigitalOut led3(LED3); 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; 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; } int main() { int count = 0; pc.baud(115200); sd.baud(9600); 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) { if(count == 50){ sd.printf("test"); count = 0; } count ++; 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)); } }