altb_pmic / AHRS

AHRS

Dependencies:   Eigen

Dependents:   IndNav_QK3_T265

AHRS.cpp@30:9b0cd3caf0ec, 2020-01-27 (annotated)

Committer:
pmic
Date:
Mon Jan 27 10:54:13 2020 +0000
Revision:
30:9b0cd3caf0ec
Parent:
28:21dfb161c67c 
Correct magnetometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 3:6811c0ce95f6 1 #include "AHRS.h"
altb2 4:3c21fb0c9e84 2
altb 3:6811c0ce95f6 3 #define PI 3.141592653589793
altb 3:6811c0ce95f6 4
altb 3:6811c0ce95f6 5 using namespace std;
altb 3:6811c0ce95f6 6
altb2 28:21dfb161c67c 7 #if _BMI088
altb2 28:21dfb161c67c 8 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), imu2(i2c), dout1(PA_10)
altb2 28:21dfb161c67c 9 #else
altb2 28:21dfb161c67c 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), dout1(PA_10)
altb2 28:21dfb161c67c 11 #endif
pmic 17:f9eed26536d9 12 {
pmic 17:f9eed26536d9 13 /* setup storage */
altb2 20:1182bc29c195 14 this->Ts = TS;
altb2 25:fe14dbcef82d 15 this->filtertype = filtertype;
altb2 25:fe14dbcef82d 16 #if _LSM9DS
altb2 25:fe14dbcef82d 17 imu_setup_LSM9DS(calib);
altb2 28:21dfb161c67c 18 #else
altb2 25:fe14dbcef82d 19 imu_setup_BMX055(calib);
altb2 28:21dfb161c67c 20 #if _BMI088
altb2 28:21dfb161c67c 21 imu_setup_BMI088(calib);
altb2 28:21dfb161c67c 22 #endif
altb2 25:fe14dbcef82d 23 #endif
altb2 25:fe14dbcef82d 24 }
altb2 26:1da7c6204775 25 // -----------------------------------------------------------------------------
altb2 25:fe14dbcef82d 26 void AHRS::imu_setup_LSM9DS(bool calib){
pmic 17:f9eed26536d9 27 float gyro[3], accel[3], magnet[3];
altb2 20:1182bc29c195 28 for(uint8_t i = 0; i < 3; i++)
altb2 20:1182bc29c195 29 {
pmic 17:f9eed26536d9 30 gyro[i] = 0.0f;
pmic 17:f9eed26536d9 31 accel[i] = 0.0f;
pmic 17:f9eed26536d9 32 magnet[i] = 0.0f;
altb2 20:1182bc29c195 33 }
pmic 17:f9eed26536d9 34
pmic 17:f9eed26536d9 35 /* inform user if imu is not responsive */
altb2 20:1182bc29c195 36 while (!imu.begin())
altb2 20:1182bc29c195 37 {
altb2 9:644266463f5f 38 wait(1);
altb2 9:644266463f5f 39 printf("Failed to communicate with LSM9DS1.\r\n");
altb2 20:1182bc29c195 40 }
pmic 17:f9eed26536d9 41
pmic 17:f9eed26536d9 42 /* take mean value of N samples */
pmic 17:f9eed26536d9 43 uint8_t N = 200;
altb2 20:1182bc29c195 44 if(calib)
altb2 20:1182bc29c195 45 {
altb2 9:644266463f5f 46 wait_ms(500);
pmic 17:f9eed26536d9 47 /* read and cumsum data */
pmic 17:f9eed26536d9 48 for(uint16_t i = 0; i < N; i++) {
pmic 17:f9eed26536d9 49 imu.readGyro();
pmic 17:f9eed26536d9 50 imu.readAccel();
pmic 17:f9eed26536d9 51 imu.readMag();
pmic 17:f9eed26536d9 52 gyro[0] += imu.gyroX;
pmic 17:f9eed26536d9 53 gyro[1] += imu.gyroY;
pmic 17:f9eed26536d9 54 gyro[2] += imu.gyroZ;
pmic 17:f9eed26536d9 55 accel[0] += imu.accX;
pmic 17:f9eed26536d9 56 accel[1] += imu.accY;
pmic 17:f9eed26536d9 57 accel[2] += imu.accZ;
pmic 17:f9eed26536d9 58 magnet[0] += imu.magX;
pmic 17:f9eed26536d9 59 magnet[1] += imu.magY;
pmic 17:f9eed26536d9 60 magnet[2] += imu.magZ;
pmic 17:f9eed26536d9 61 wait_ms(10);
pmic 17:f9eed26536d9 62 }
pmic 17:f9eed26536d9 63 /* calculate mean value */
pmic 17:f9eed26536d9 64 for(uint16_t i = 0; i < 3; i++) {
pmic 17:f9eed26536d9 65 gyro[i] /= (float)N;
pmic 17:f9eed26536d9 66 accel[i] /= (float)N;
pmic 17:f9eed26536d9 67 magnet[i] /= (float)N;
altb2 20:1182bc29c195 68 }
pmic 17:f9eed26536d9 69 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]);
pmic 17:f9eed26536d9 70 }
pmic 17:f9eed26536d9 71
pmic 17:f9eed26536d9 72 /* gyro */
altb2 26:1da7c6204775 73 // ========== ACHTUNG HIER LSM9DS SENSOR
pmic 17:f9eed26536d9 74 raw_gx2gx.setup( 1.0, gyro[0]);
pmic 17:f9eed26536d9 75 raw_gy2gy.setup(-1.0, gyro[1]); // y-axis reversed (lefthanded system)
pmic 17:f9eed26536d9 76 raw_gz2gz.setup( 1.0, gyro[2]);
pmic 17:f9eed26536d9 77
pmic 17:f9eed26536d9 78 // pmic, 23.09.2019
pmic 17:f9eed26536d9 79 // calibration for acc and mag needs to be redone on real copter pes board with power on and spinning motors (no propellers)
pmic 17:f9eed26536d9 80 /* accel */
altb2 19:42ea6dd68185 81 raw_ax2ax.setup( 1.0f, accel[0]); // use gain and offset here
altb2 19:42ea6dd68185 82 raw_ay2ay.setup(-1.0f, accel[1]); // y-axis reversed // was -1,0.0
altb2 19:42ea6dd68185 83 raw_az2az.setup( 1.0, 0.0f); // do not remove gravity!
altb2 26:1da7c6204775 84 // ========== ACHTUNG HIER LSM9DS SENSOR
pmic 17:f9eed26536d9 85 /* magnet */
altb2 21:31e01d3e0143 86 /* Caibration on 21.10.2019 QK2 delivers:
altb2 19:42ea6dd68185 87 A=[ 0.9904 0 0; 0.0605 1.0158 0;0.0397 -0.0199 0.9938]
altb2 21:31e01d3e0143 88 b0= [0.4160 0.1587 -0.0998], //see T:\T-IMS-IndNav\01_Technisches\09_PES_Board_IMU\meas_mag_aFMA\calib_magn_Okt19_aFMA.m
altb2 19:42ea6dd68185 89 */
altb2 23:71996bfe68eb 90 raw_mx2mx.setup( 0.9837f,0.252f);
altb2 23:71996bfe68eb 91 raw_my2my.setup( 1.0298f,0.1675f);
altb2 23:71996bfe68eb 92 raw_mz2mz.setup(-0.9864f,-0.1392f); // achtung, bei gain(3) Vorzeichen drehen, b0 beibehalten!!!
altb2 23:71996bfe68eb 93 // raw_mx2mx.setup( 1.0f,0.0f);
altb2 23:71996bfe68eb 94 // raw_my2my.setup( 1.0f,0.0f);
altb2 23:71996bfe68eb 95 // raw_mz2mz.setup( 1.0f,0.0f); // achtung, bei b0(3) Vorzeichen drehen!!!
altb2 23:71996bfe68eb 96
pmic 17:f9eed26536d9 97 this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
pmic 17:f9eed26536d9 98 this->magnet_cal_0[1] = raw_my2my(magnet[1]);
pmic 17:f9eed26536d9 99 this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
altb2 21:31e01d3e0143 100 // set EKF_RPY magnet values now:
altb2 23:71996bfe68eb 101 ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
altb2 7:bfde7bd5fe31 102 local_time = 0.0;
altb2 19:42ea6dd68185 103
pmic 17:f9eed26536d9 104 // the thread starts
altb2 9:644266463f5f 105
altb 3:6811c0ce95f6 106 }
altb2 25:fe14dbcef82d 107 //------------------------------------------------------------------------------
altb2 25:fe14dbcef82d 108 void AHRS::imu_setup_BMX055(bool calib){
altb2 25:fe14dbcef82d 109 float gyro[3], accel[3], magnet[3];
altb2 25:fe14dbcef82d 110 for(uint8_t i = 0; i < 3; i++)
altb2 25:fe14dbcef82d 111 {
altb2 25:fe14dbcef82d 112 gyro[i] = 0.0f;
altb2 25:fe14dbcef82d 113 accel[i] = 0.0f;
altb2 25:fe14dbcef82d 114 magnet[i] = 0.0f;
altb2 25:fe14dbcef82d 115 }
altb2 25:fe14dbcef82d 116
altb2 25:fe14dbcef82d 117 /* take mean value of N samples */
altb2 25:fe14dbcef82d 118 uint8_t N = 200;
altb2 25:fe14dbcef82d 119 if(calib)
altb2 25:fe14dbcef82d 120 {
altb2 25:fe14dbcef82d 121 wait_ms(500);
altb2 25:fe14dbcef82d 122 /* read and cumsum data */
altb2 25:fe14dbcef82d 123 for(uint16_t i = 0; i < N; i++) {
altb2 25:fe14dbcef82d 124 imu.readGyro();
altb2 25:fe14dbcef82d 125 imu.readAccel();
altb2 25:fe14dbcef82d 126 imu.readMag();
altb2 25:fe14dbcef82d 127 gyro[0] += imu.gyroX;
altb2 25:fe14dbcef82d 128 gyro[1] += imu.gyroY;
altb2 25:fe14dbcef82d 129 gyro[2] += imu.gyroZ;
altb2 25:fe14dbcef82d 130 accel[0] += imu.accX;
altb2 25:fe14dbcef82d 131 accel[1] += imu.accY;
altb2 25:fe14dbcef82d 132 accel[2] += imu.accZ;
altb2 25:fe14dbcef82d 133 magnet[0] += imu.magX;
altb2 25:fe14dbcef82d 134 magnet[1] += imu.magY;
altb2 25:fe14dbcef82d 135 magnet[2] += imu.magZ;
altb2 28:21dfb161c67c 136 wait_ms(5);
altb2 25:fe14dbcef82d 137 }
altb2 25:fe14dbcef82d 138 /* calculate mean value */
altb2 25:fe14dbcef82d 139 for(uint16_t i = 0; i < 3; i++) {
altb2 25:fe14dbcef82d 140 gyro[i] /= (float)N;
altb2 25:fe14dbcef82d 141 accel[i] /= (float)N;
altb2 25:fe14dbcef82d 142 magnet[i] /= (float)N;
altb2 25:fe14dbcef82d 143 }
altb2 25:fe14dbcef82d 144 }
altb2 26:1da7c6204775 145 // ========== ACHTUNG HIER BMX055 SENSOR
altb2 25:fe14dbcef82d 146 /* gyro */
altb2 25:fe14dbcef82d 147 raw_gx2gx.setup( 1.0, gyro[0]);
altb2 25:fe14dbcef82d 148 raw_gy2gy.setup( 1.0, gyro[1]); // y-axis reversed (lefthanded system)
altb2 25:fe14dbcef82d 149 raw_gz2gz.setup( 1.0, gyro[2]);
altb2 25:fe14dbcef82d 150
altb2 25:fe14dbcef82d 151 /* accel */
altb2 25:fe14dbcef82d 152 raw_ax2ax.setup( 1.0f, accel[0]); // use gain and offset here
altb2 28:21dfb161c67c 153 raw_ay2ay.setup( 1.0f, accel[1]); //
altb2 25:fe14dbcef82d 154 raw_az2az.setup( 1.0, 0.0f); // do not remove gravity!
altb2 25:fe14dbcef82d 155
altb2 26:1da7c6204775 156 // ========== ACHTUNG HIER BMX055 SENSOR
altb2 25:fe14dbcef82d 157 /* magnet */
altb2 25:fe14dbcef82d 158 // diag(A0) = 0.8047 0.8447 0.8474
altb2 26:1da7c6204775 159 // diag(A0) = 0.7485 0.8074 0.8892 //10.1.2020
altb2 25:fe14dbcef82d 160 // b0 = -0.1195 0.2009 -0.3133, calibration at desk 19.12.2019
altb2 26:1da7c6204775 161 // b0 = 0.2656 0.4564 0.9155 // 10.1.20
altb2 26:1da7c6204775 162 // 1.0150 1.0843 1.1814, 0.2826 0.4353 0.9392 aFMA 10.1.2020
altb2 26:1da7c6204775 163 raw_mx2mx.setup(1.0150f,0.2826f);
altb2 26:1da7c6204775 164 raw_my2my.setup(1.0843f,0.4353f);
altb2 26:1da7c6204775 165 raw_mz2mz.setup(1.1814f,0.9392f); // */
altb2 25:fe14dbcef82d 166 /* raw_mx2mx.setup( 1.0f,0.0f);
altb2 25:fe14dbcef82d 167 raw_my2my.setup( 1.0f,0.0f);
altb2 26:1da7c6204775 168 raw_mz2mz.setup( 1.0f,0.0f); // */
altb2 25:fe14dbcef82d 169 this->magnet_cal_0[0] = raw_mx2mx(magnet[0]);
altb2 25:fe14dbcef82d 170 this->magnet_cal_0[1] = raw_my2my(magnet[1]);
altb2 25:fe14dbcef82d 171 this->magnet_cal_0[2] = raw_mz2mz(magnet[2]);
altb2 25:fe14dbcef82d 172 // set EKF_RPY magnet values now:
altb2 25:fe14dbcef82d 173 ekf_rpy.set_m0(this->magnet_cal_0[0],this->magnet_cal_0[1],this->magnet_cal_0[2]);
altb2 25:fe14dbcef82d 174 local_time = 0.0;
altb2 28:21dfb161c67c 175 printf("initialized BMX055\r\n");
altb2 25:fe14dbcef82d 176
altb2 25:fe14dbcef82d 177 // the thread starts
altb2 25:fe14dbcef82d 178
altb2 25:fe14dbcef82d 179 }
altb2 28:21dfb161c67c 180 //------------------------------------------------------------------------------
altb2 28:21dfb161c67c 181 #if _BMI088
altb2 28:21dfb161c67c 182 void AHRS::imu_setup_BMI088(bool calib){
altb2 28:21dfb161c67c 183 float gyro[3], accel[3];
altb2 28:21dfb161c67c 184 for(uint8_t i = 0; i < 3; i++)
altb2 28:21dfb161c67c 185 {
altb2 28:21dfb161c67c 186 gyro[i] = 0.0f;
altb2 28:21dfb161c67c 187 accel[i] = 0.0f;
altb2 28:21dfb161c67c 188 }
altb2 25:fe14dbcef82d 189
altb2 28:21dfb161c67c 190 /* take mean value of N samples */
altb2 28:21dfb161c67c 191 uint8_t N = 200;
altb2 28:21dfb161c67c 192 if(calib)
altb2 28:21dfb161c67c 193 {
altb2 28:21dfb161c67c 194 wait_ms(500);
altb2 28:21dfb161c67c 195 /* read and cumsum data */
altb2 28:21dfb161c67c 196 for(uint16_t i = 0; i < N; i++) {
altb2 28:21dfb161c67c 197 imu2.readGyro();
altb2 28:21dfb161c67c 198 imu2.readAccel();
altb2 28:21dfb161c67c 199 gyro[0] += imu2.gyroX;
altb2 28:21dfb161c67c 200 gyro[1] += imu2.gyroY;
altb2 28:21dfb161c67c 201 gyro[2] += imu2.gyroZ;
altb2 28:21dfb161c67c 202 accel[0] += imu2.accX;
altb2 28:21dfb161c67c 203 accel[1] += imu2.accY;
altb2 28:21dfb161c67c 204 accel[2] += imu2.accZ;
altb2 28:21dfb161c67c 205 wait_ms(5);
altb2 28:21dfb161c67c 206 }
altb2 28:21dfb161c67c 207 /* calculate mean value */
altb2 28:21dfb161c67c 208 for(uint16_t i = 0; i < 3; i++) {
altb2 28:21dfb161c67c 209 gyro[i] /= (float)N;
altb2 28:21dfb161c67c 210 accel[i] /= (float)N;
altb2 28:21dfb161c67c 211 }
altb2 28:21dfb161c67c 212 }
altb2 28:21dfb161c67c 213 // ========== ACHTUNG HIER BMI088 SENSOR
altb2 28:21dfb161c67c 214 /* gyro */
altb2 28:21dfb161c67c 215 raw_gx2gx88.setup( 1.0, gyro[0]);
altb2 28:21dfb161c67c 216 raw_gy2gy88.setup( 1.0, gyro[1]); //
altb2 28:21dfb161c67c 217 raw_gz2gz88.setup( 1.0, gyro[2]);
altb2 28:21dfb161c67c 218
altb2 28:21dfb161c67c 219 /* accel */
altb2 28:21dfb161c67c 220 raw_ax2ax88.setup( 1.0f, accel[0]); // use gain and offset here
altb2 28:21dfb161c67c 221 raw_ay2ay88.setup( 1.0f, accel[1]); //
altb2 28:21dfb161c67c 222 raw_az2az88.setup( 1.0, 0.0f); // do not remove gravity!
altb2 28:21dfb161c67c 223 printf("initialized BMI088\r\n");
altb2 28:21dfb161c67c 224 }
altb2 28:21dfb161c67c 225 #endif
altb 3:6811c0ce95f6 226
altb 3:6811c0ce95f6 227 AHRS::~AHRS() {}
altb 3:6811c0ce95f6 228
altb 3:6811c0ce95f6 229 void AHRS::update(void)
altb2 20:1182bc29c195 230 {
altb2 28:21dfb161c67c 231 //
altb2 21:31e01d3e0143 232 switch(filtertype){
altb2 21:31e01d3e0143 233 case 1:
altb2 21:31e01d3e0143 234 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]);
altb2 22:495a419e474c 235 cf_yaw.update(data.sens_gyr[2],data.sens_mag[0],data.sens_mag[1],data.sens_mag[2]);
altb2 21:31e01d3e0143 236 data.est_RPY[0] = ekf.get_est_state(0);
altb2 21:31e01d3e0143 237 data.est_RPY[1] = ekf.get_est_state(1);
altb2 23:71996bfe68eb 238 //data.est_Vxyz[0] = ekf.get_est_state(4);
altb2 23:71996bfe68eb 239 //data.est_Vxyz[1] = ekf.get_est_state(5);
altb2 21:31e01d3e0143 240 break;
altb2 21:31e01d3e0143 241 case 2:
altb2 21:31e01d3e0143 242 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],0.0f,0.0f,0.0f);
altb2 21:31e01d3e0143 243 data.est_RPY[0] = Mahony_filter.getRollRadians();
altb2 21:31e01d3e0143 244 data.est_RPY[1] = Mahony_filter.getPitchRadians();
altb2 21:31e01d3e0143 245 break;
altb2 21:31e01d3e0143 246 case 3:
altb2 21:31e01d3e0143 247 ekf_rp.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_acc[0],data.sens_acc[1]);
altb2 23:71996bfe68eb 248
altb2 23:71996bfe68eb 249 cf_yaw.update(data.sens_gyr[2],data.sens_mag[0],data.sens_mag[1],data.sens_mag[2]);
altb2 21:31e01d3e0143 250 data.est_RPY[0] = ekf_rp.get_est_state(0);
altb2 21:31e01d3e0143 251 data.est_RPY[1] = ekf_rp.get_est_state(1);
altb2 21:31e01d3e0143 252 break;
altb2 21:31e01d3e0143 253 case 4:
altb2 21:31e01d3e0143 254 ekf_rpy.update(data.sens_gyr[0],data.sens_gyr[1],data.sens_gyr[2],data.sens_acc[0],data.sens_acc[1],data.sens_mag[0],data.sens_mag[1]);
altb2 21:31e01d3e0143 255 data.est_RPY[0] = ekf_rpy.get_est_state(0);
altb2 21:31e01d3e0143 256 data.est_RPY[1] = ekf_rpy.get_est_state(1);
altb2 23:71996bfe68eb 257 data.est_RPY[2] = ekf_rpy.get_est_state(2);
altb2 23:71996bfe68eb 258 data.est_RPY[3] = ekf_rpy.get_est_state(3);
altb2 23:71996bfe68eb 259 data.est_RPY[4] = ekf_rpy.get_est_state(4);
altb2 23:71996bfe68eb 260 data.est_RPY[5] = ekf_rpy.get_est_state(5);
altb2 23:71996bfe68eb 261 data.est_RPY[6] = ekf_rpy.get_est_state(6);
altb2 23:71996bfe68eb 262 data.est_RPY[7] = ekf_rpy.get_est_state(7);
altb2 21:31e01d3e0143 263 break;
altb2 20:1182bc29c195 264 }
altb2 20:1182bc29c195 265 // dout3.write(0);
altb2 20:1182bc29c195 266 }
altb2 20:1182bc29c195 267 void AHRS::update_as_thread(void)
altb2 20:1182bc29c195 268 {
altb2 20:1182bc29c195 269 while(1)
altb2 20:1182bc29c195 270 {
altb2 20:1182bc29c195 271 thread.signal_wait(signal);
altb2 20:1182bc29c195 272 // dout3.write(1);
altb2 20:1182bc29c195 273 if(filtertype !=1)
altb2 20:1182bc29c195 274 {
altb2 20:1182bc29c195 275 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],0.0f,0.0f,0.0f);
altb2 20:1182bc29c195 276 data.est_RPY[0] = Mahony_filter.getRollRadians();
altb2 20:1182bc29c195 277 data.est_RPY[1] = Mahony_filter.getPitchRadians();
altb2 20:1182bc29c195 278 }
altb2 20:1182bc29c195 279 else
altb2 20:1182bc29c195 280 {
altb2 20:1182bc29c195 281 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]);
altb2 20:1182bc29c195 282 data.est_RPY[0] = ekf.get_est_state(0);
altb2 20:1182bc29c195 283 data.est_RPY[1] = ekf.get_est_state(1);
altb2 20:1182bc29c195 284 }
altb2 20:1182bc29c195 285 // dout3.write(0);
altb2 20:1182bc29c195 286 }
altb2 20:1182bc29c195 287 }
altb2 20:1182bc29c195 288 void AHRS::read_imu_sensors(void){
altb2 28:21dfb161c67c 289 // mutex.lock();
altb2 28:21dfb161c67c 290 dout1.write(1);
altb2 20:1182bc29c195 291 imu.readGyro();
altb2 20:1182bc29c195 292 imu.readAccel();
altb2 20:1182bc29c195 293 imu.readMag();
altb2 28:21dfb161c67c 294 dout1.write(0);
altb2 20:1182bc29c195 295 data.sens_gyr[0] = raw_gx2gx(imu.gyroX);
altb2 20:1182bc29c195 296 data.sens_gyr[1] = raw_gy2gy(imu.gyroY);
altb2 20:1182bc29c195 297 data.sens_gyr[2] = raw_gz2gz(imu.gyroZ);
altb2 20:1182bc29c195 298 data.sens_acc[0] = raw_ax2ax(imu.accX);
altb2 20:1182bc29c195 299 data.sens_acc[1] = raw_ay2ay(imu.accY);
altb2 20:1182bc29c195 300 data.sens_acc[2] = raw_az2az(imu.accZ);
altb2 20:1182bc29c195 301 data.sens_mag[0] = raw_mx2mx(imu.magX);
altb2 20:1182bc29c195 302 data.sens_mag[1] = raw_my2my(imu.magY);
altb2 20:1182bc29c195 303 data.sens_mag[2] = raw_mz2mz(imu.magZ);
altb2 28:21dfb161c67c 304 // mutex.unlock();
altb2 28:21dfb161c67c 305 #if _BMI088
altb2 28:21dfb161c67c 306 imu2.readGyro();
altb2 28:21dfb161c67c 307 imu2.readAccel();
altb2 28:21dfb161c67c 308 data.sens_gyr[3] = raw_gx2gx88(imu2.gyroX);
altb2 28:21dfb161c67c 309 data.sens_gyr[4] = raw_gy2gy88(imu2.gyroY);
altb2 28:21dfb161c67c 310 data.sens_gyr[5] = raw_gz2gz88(imu2.gyroZ);
altb2 28:21dfb161c67c 311 data.sens_acc[3] = raw_ax2ax88(imu2.accX);
altb2 28:21dfb161c67c 312 data.sens_acc[4] = raw_ay2ay88(imu2.accY);
altb2 28:21dfb161c67c 313 data.sens_acc[5] = raw_az2az88(imu2.accZ);
altb2 28:21dfb161c67c 314 #endif
altb2 20:1182bc29c195 315 }
altb2 20:1182bc29c195 316 // ------------------- start controllers ----------------
altb2 20:1182bc29c195 317 void AHRS::start_loop(void){
altb2 20:1182bc29c195 318 thread.start(callback(this, &AHRS::update_as_thread));
altb2 20:1182bc29c195 319 ticker.attach(callback(this, &AHRS::sendSignal), Ts);
altb2 20:1182bc29c195 320 }
pmic 17:f9eed26536d9 321
altb2 20:1182bc29c195 322 // this is for realtime OS
altb2 20:1182bc29c195 323 void AHRS::sendSignal() {
altb2 20:1182bc29c195 324 thread.signal_set(signal);
altb2 20:1182bc29c195 325 }