AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 17:f9eed26536d9
- Parent:
- 14:c357be9a3fc8
- Child:
- 19:42ea6dd68185
diff -r 97c12f818b94 -r f9eed26536d9 AHRS.cpp --- a/AHRS.cpp Mon Sep 23 09:53:03 2019 +0000 +++ b/AHRS.cpp Mon Sep 23 14:19:23 2019 +0000 @@ -7,75 +7,100 @@ 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){ - float g[3]; - g[0]=0.0;g[1]=0.0;g[2]=0.0; - float a[3]; - a[0]=0.0;a[1]=0.0;a[2]=0.0; - uint8_t N = 100; +//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) +{ + /* setup storage */ + 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; + } + + /* inform user if imu is not responsive */ while (!imu.begin()) { wait(1); printf("Failed to communicate with LSM9DS1.\r\n"); } - if(calib){ + + /* take mean value of N samples */ + uint8_t N = 200; + if(calib) { wait_ms(500); - for(int k=0;k < N;k++) - { - imu.readGyro(); - imu.readAccel(); - g[0]+= imu.gyroX; - g[1]+= imu.gyroY; - g[2]+= imu.gyroZ; - a[0]+= imu.accX; - a[1]+= imu.accY; - a[2]+= imu.accZ; - wait_ms(10); - } - for(int k = 0;k<3;k++){ - g[k] /= (float)N; - a[k] /= (float)N; - } - printf("Correct g: %1.5f %1.5f %1.5f a: %1.5f %1.5f %1.5f\r\n",g[0],g[1],g[2],a[0],a[1],a[2]); + /* 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; } - raw_gx2gx.setup( 1.0,g[0]); - raw_gy2gy.setup(-1.0,g[1]); // y-axis reversed (lefthanded system) - raw_gz2gz.setup( 1.0,g[2]); - raw_ax2ax.setup( 1.0,a[0]); // use gain and offset here - raw_ay2ay.setup(-1.0,a[1]); // y-axis reversed // was -1,0.0 - raw_az2az.setup( 1.0,0.0); - int2magx.setup( -1.0,0.0); // x-axis reversed - int2magy.setup( -1.0,0.0); // y-axis reversed - int2magz.setup( 1.0,0.0); + printf("Correct gyro: %1.5f %1.5f %1.5f accel: %1.5f %1.5f %1.5f magnet: %1.5f %1.5f %1.5f\r\n", gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnet[0], magnet[1], magnet[2]); + } + + /* 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]); + + // pmic, 23.09.2019 + // 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! + + /* magnet */ + raw_mx2mx.setup( 0.9708198f, -0.1047768f); + raw_my2my.setup( 1.0038431f, 0.2345627f); + raw_mz2mz.setup(-1.0261139f, 0.0650053f); + 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; -// The Thread starts + + // the thread starts thread.start(callback(this, &AHRS::update)); ticker.attach(callback(this, &AHRS::sendSignal), TS); - - } AHRS::~AHRS() {} - void AHRS::update(void) { - while(1){ + while(1) { + thread.signal_wait(signal); + imu.readGyro(); imu.readAccel(); - //imu.readMag_calibrated(); imu.readMag(); - imu.readGyro(); - matrix measurement(6,1,0.0); + 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)); + 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); @@ -88,10 +113,18 @@ 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); - } // while(1) (the thread) - + + // 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); + + } // while(1) (the thread) + } -void AHRS::sendSignal() { - + +void AHRS::sendSignal() +{ thread.signal_set(signal); } \ No newline at end of file