AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 19:42ea6dd68185
- Parent:
- 17:f9eed26536d9
- Child:
- 20:1182bc29c195
diff -r e374009ba477 -r 42ea6dd68185 AHRS.cpp --- a/AHRS.cpp Mon Sep 23 14:23:11 2019 +0000 +++ b/AHRS.cpp Wed Oct 02 15:30:15 2019 +0000 @@ -8,7 +8,7 @@ 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), RPY_filter(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096) +AHRS::AHRS(uint8_t filtertype, float TS, bool calib) : imu(PC_9, PA_8, 0xD6, 0x3C), ekf(TS), Mahony_filter(TS), thread(osPriorityBelowNormal, 4096) { /* setup storage */ float gyro[3], accel[3], magnet[3]; @@ -62,20 +62,25 @@ // calibration for acc and mag needs to be redone on real copter pes board with power on and spinning motors (no propellers) /* accel */ - raw_ax2ax.setup( 1.0032920, accel[0]); // use gain and offset here - raw_ay2ay.setup(-0.9854960, accel[1]); // y-axis reversed // was -1,0.0 - raw_az2az.setup( 1.0113879, 0.0f); // do not remove gravity! + 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 */ - raw_mx2mx.setup( 0.9708198f, -0.1047768f); - raw_my2my.setup( 1.0038431f, 0.2345627f); - raw_mz2mz.setup(-1.0261139f, 0.0650053f); + /* Caibration on 25.9.2019 QK2 delivers: + A=[ 0.9904 0 0; 0.0605 1.0158 0;0.0397 -0.0199 0.9938] + b0= [0.4160 0.1587 -0.0998], see T:\T-IMS-IndNav\01_Technisches\09_PES_Board_IMU\meas_mag_aFMA\calib_mag_QK2.m + */ + raw_mx2mx.setup( 0.9904f,0.4160f); + raw_my2my.setup( 1.0158f, 0.1587f); + raw_mz2mz.setup(-0.9938f, -0.0998f); 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]); local_time = 0.0; - + + this->filtertype = filtertype; // the thread starts thread.start(callback(this, &AHRS::update)); ticker.attach(callback(this, &AHRS::sendSignal), TS); @@ -86,45 +91,11 @@ void AHRS::update(void) { - while(1) { - - thread.signal_wait(signal); - imu.readGyro(); - imu.readAccel(); - imu.readMag(); - matrix measurement(6, 1, 0.0f); - //Perform filter update - //Mahony_filter.update(raw_gx2gx(imu.gyroX), raw_gy2gy(imu.gyroY), raw_gz2gz(imu.gyroZ) , - // raw_ax2ax(imu.accX), raw_ay2ay(imu.accY), raw_az2az(imu.accZ), - // int2magx(imu.magX), int2magy(imu.magY), int2magz(imu.magZ)); - measurement.put_entry(0, 0, raw_gx2gx(imu.gyroX)); - measurement.put_entry(1, 0, raw_gy2gy(imu.gyroY)); - measurement.put_entry(3, 0, raw_ax2ax(imu.accX)); - measurement.put_entry(4, 0, raw_ay2ay(imu.accY)); - RPY_filter.loop(&measurement); - - my_logger.data_vector[0] = raw_gx2gx(imu.gyroX); - my_logger.data_vector[1] = raw_gy2gy(imu.gyroY); - my_logger.data_vector[2] = raw_gz2gz(imu.gyroZ); - my_logger.data_vector[3] = raw_ax2ax(imu.accX); - my_logger.data_vector[4] = raw_ay2ay(imu.accY); - my_logger.data_vector[5] = raw_az2az(imu.accZ); - my_logger.data_vector[6] = RPY_filter.get_est_state(0); - my_logger.data_vector[7] = RPY_filter.get_est_state(1); - my_logger.data_vector[8] = RPY_filter.get_est_state(4); - my_logger.data_vector[9] = RPY_filter.get_est_state(5); - - // pmic, 23.09.2019 - // dono about logger extension, please adapt - // my_logger.data_vector[10] = raw_mx2mx(imu.magX); - // my_logger.data_vector[11] = raw_my2my(imu.magY); - // my_logger.data_vector[12] = raw_mz2mz(imu.magZ); + if(filtertype !=1) + Mahony_filter.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]); + else + ekf.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_acc[2]); } // while(1) (the thread) } - -void AHRS::sendSignal() -{ - thread.signal_set(signal); -} \ No newline at end of file