altb_pmic
AHRS
Dependencies: Eigen
Diff: AHRS.cpp
- Revision:
- 17:f9eed26536d9
- Parent:
- 14:c357be9a3fc8
- Child:
- 19:42ea6dd68185
--- 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