AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 25:fe14dbcef82d
- Parent:
- 23:71996bfe68eb
- Child:
- 26:1da7c6204775
diff -r 71996bfe68eb -r fe14dbcef82d AHRS.cpp --- a/AHRS.cpp Wed Nov 13 11:40:46 2019 +0000 +++ b/AHRS.cpp Mon Jan 06 12:49:38 2020 +0000 @@ -5,10 +5,18 @@ using namespace std; //OLD: AHRS::AHRS(uint8_t filtertype, float TS) : RPY_filter(TS), csAG(PA_15),csM(PD_2), spi(PC_12, PC_11, PC_10), imu(&spi, &csM, &csAG), thread(osPriorityBelowNormal, 4096){ -AHRS::AHRS(uint8_t filtertype, float TS, bool calib) : imu(PC_9, PA_8, 0xD6, 0x3C), ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS)//, dout3(PA_10) +AHRS::AHRS(uint8_t filtertype, float TS, bool calib, I2C &i2c) : ekf(TS), Mahony_filter(TS), cf_yaw(1.0f,TS), ekf_rp(TS), ekf_rpy(TS), imu(i2c), dout3(PA_10) { /* setup storage */ this->Ts = TS; + this->filtertype = filtertype; + #if _LSM9DS + imu_setup_LSM9DS(calib); + #else + imu_setup_BMX055(calib); + #endif +} +void AHRS::imu_setup_LSM9DS(bool calib){ float gyro[3], accel[3], magnet[3]; for(uint8_t i = 0; i < 3; i++) { @@ -86,10 +94,78 @@ ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]); local_time = 0.0; - this->filtertype = filtertype; // the thread starts } +//------------------------------------------------------------------------------ +void AHRS::imu_setup_BMX055(bool calib){ + float gyro[3], accel[3], magnet[3]; + for(uint8_t i = 0; i < 3; i++) + { + gyro[i] = 0.0f; + accel[i] = 0.0f; + magnet[i] = 0.0f; + } + + /* take mean value of N samples */ + uint8_t N = 200; + if(calib) + { + wait_ms(500); + /* read and cumsum data */ + for(uint16_t i = 0; i < N; i++) { + imu.readGyro(); + imu.readAccel(); + imu.readMag(); + gyro[0] += imu.gyroX; + gyro[1] += imu.gyroY; + gyro[2] += imu.gyroZ; + accel[0] += imu.accX; + accel[1] += imu.accY; + accel[2] += imu.accZ; + magnet[0] += imu.magX; + magnet[1] += imu.magY; + magnet[2] += imu.magZ; + wait_ms(10); + } + /* calculate mean value */ + for(uint16_t i = 0; i < 3; i++) { + gyro[i] /= (float)N; + accel[i] /= (float)N; + magnet[i] /= (float)N; + } + } + + /* gyro */ + raw_gx2gx.setup( 1.0, gyro[0]); + raw_gy2gy.setup( 1.0, gyro[1]); // y-axis reversed (lefthanded system) + raw_gz2gz.setup( 1.0, gyro[2]); + + /* accel */ + raw_ax2ax.setup( 1.0f, accel[0]); // use gain and offset here + raw_ay2ay.setup( 1.0f, accel[1]); // y-axis reversed // was -1,0.0 + raw_az2az.setup( 1.0, 0.0f); // do not remove gravity! + + /* magnet */ + // diag(A0) = 0.8047 0.8447 0.8474 + // b0 = -0.1195 0.2009 -0.3133, calibration at desk 19.12.2019 + raw_mx2mx.setup( 0.8047f,-0.1195f); + raw_my2my.setup( 0.8447f, 0.2009f); + raw_mz2mz.setup( 0.8474f,-0.3133f); // +/* raw_mx2mx.setup( 1.0f,0.0f); + raw_my2my.setup( 1.0f,0.0f); + raw_mz2mz.setup( 1.0f,0.0f); */ + this->magnet_cal_0[0] = raw_mx2mx(magnet[0]); + this->magnet_cal_0[1] = raw_my2my(magnet[1]); + this->magnet_cal_0[2] = raw_mz2mz(magnet[2]); +// set EKF_RPY magnet values now: + ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]); + local_time = 0.0; + + // the thread starts + +} + AHRS::~AHRS() {} @@ -153,9 +229,13 @@ } } void AHRS::read_imu_sensors(void){ + mutex.lock(); + dout3.write(1); imu.readGyro(); imu.readAccel(); imu.readMag(); + dout3.write(0); + mutex.unlock(); data.sens_gyr[0] = raw_gx2gx(imu.gyroX); data.sens_gyr[1] = raw_gy2gy(imu.gyroY); data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);