void IMU::mahonyStepAction(void) {
const float32_t * const pGyro = gyro.axisInRadPerS, * const pAcc = accelerometer.axisInMPerSsquared, * const pMag = magnetometer.axis_f;
MahonyAHRSupdate(pGyro[0], pGyro[1], pGyro[2],
pAcc[0], pAcc[1], pAcc[2],
pMag[0], pMag[1], pMag[2]);
MahonyToEuler(eulerAnglesInRadMahony);
}
/*
* Output functions
*
*/
void ahrsFction_Outputs_wrapper(const real32_T *accIn,
const real32_T *gyroIn,
const real32_T *magIn,
const real32_T *Kp,
const real32_T *Ki,
real32_T *quat,
const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
// //Madgwick
// if(xD[0] == 0) {
// q0 = 1;
// q1 = 0;
// q2 = 0;
// q3 = 0;
// } else {
// MadgwickAHRSupdate(gyroIn[0],gyroIn[1],gyroIn[2],accIn[0],accIn[1],accIn[2],magIn[0],magIn[1],magIn[2]);
// // MadgwickAHRSupdateIMU(gyroIn[0],gyroIn[1],gyroIn[2],accIn[0],accIn[1],accIn[2]);
// }
// quat[0] = q0;
// quat[1] = q1;
// quat[2] = q2;
// quat[3] = q3;
// Mahony
if(xD[0] == 0) {
q0 = 1;
q1 = 0;
q2 = 0;
q3 = 0;
twoKp = 2*Kp[0];
twoKi = 2*Ki[0];
} else {
MahonyAHRSupdate(gyroIn[0],gyroIn[1],gyroIn[2],accIn[0],accIn[1],accIn[2],magIn[0],magIn[1],magIn[2]);
// MahonyAHRSupdateIMU(gyroIn[0],gyroIn[1],gyroIn[2],accIn[0],accIn[1],accIn[2]);
}
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
// //algorytm
// if(xD[0] == 0) {
// filterInit();
// } else {
// filterUpdate(gyroIn[0],gyroIn[1],gyroIn[2],accIn[0],accIn[1],accIn[2]);
// quat[0] = SEq_1;
// quat[1] = SEq_2;
// quat[2] = SEq_3;
// quat[3] = SEq_4;
// }
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
void Attitude::update() {
float magx, magy, magz;
m_mag.getValues(&magx, &magy, &magz);
int16_t ax, ay, az, gx, gy, gz;
m_ags.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gx += m_config->cal.gyro_offset.x;
gy += m_config->cal.gyro_offset.y;
gz += m_config->cal.gyro_offset.z;
const float fgx = float(gx)/1877.4681031;
const float fgy = float(gy)/1877.4681031;
const float fgz = float(gz)/1877.4681031;
const float fax = ax + m_config->cal.acc_offset.x;
const float fay = ay + m_config->cal.acc_offset.y;
const float faz = az + m_config->cal.acc_offset.z;
MahonyAHRSupdate(fgx, fgy, fgz, fax, fay, faz, magx, magy, magz);
const float px = 2 * (q1*q3 - q0*q2);
const float py = 2 * (q0*q1 + q2*q3);
const float pz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
yaw = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0*q0 + 2 * q1 * q1 - 1) * 180/M_PI;
pitch = atan2(px, sqrt(py*py + pz*pz)) * 180/M_PI;
roll = atan2(py, sqrt(px*px + pz*pz)) * 180/M_PI;
// Every 1/2 second update the sample rate for the gyro calculations
++m_counter;
const uint32_t end = micros();
const uint32_t total = end - m_startTime;
if (total > 100000) {
sampleFreq = m_counter*10;
m_counter = 0;
m_startTime = end;
}
}
void estimate_pos_mahony(double time){
quaternion_t quaternion;
vector3d_t euler;
int i;
double timedelay = (time-timeOld);
unsigned char data_read[6];
//read accel
i2c_read(0x69, 0x3B, 6, data_read);
rawAccel[VEC3_X] = ((short)data_read[0] << 8) | data_read[1];
rawAccel[VEC3_Y] = ((short)data_read[2] << 8) | data_read[3];
rawAccel[VEC3_Z] = ((short)data_read[4] << 8) | data_read[5];
//read gyro
i2c_read(0x69, 0x43, 6, data_read);
rawGyro[VEC3_X] = ((short)data_read[0] << 8) | data_read[1];
rawGyro[VEC3_Y] = ((short)data_read[2] << 8) | data_read[3];
rawGyro[VEC3_Z] = ((short)data_read[4] << 8) | data_read[5];
for(i=VEC3_X;i<(VEC3_Z+1);i++){
accel[i] = (float)rawAccel[i] / (ACCSENS);
gyro[i] = (float)rawGyro[i] * DEGREE_TO_RAD / (GYROSENS);
}
MahonyAHRSupdate(gyro[VEC3_X],gyro[VEC3_Y],gyro[VEC3_Z],accel[VEC3_X],accel[VEC3_Y],accel[VEC3_Z],0,0,0);
quaternion[QUAT_W] = q0;
quaternion[QUAT_X] = q1;
quaternion[QUAT_Y] = q2;
quaternion[QUAT_Z] = q3;
quaternionToEuler(quaternion,euler);
// printf("%f %f %f | %f\n",euler[VEC3_X]*RAD_TO_DEGREE,euler[VEC3_Y]*RAD_TO_DEGREE,euler[VEC3_Z]*RAD_TO_DEGREE, timedelay);
printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", gyro[VEC3_X], gyro[VEC3_Y], gyro[VEC3_Z], accel[VEC3_X], accel[VEC3_Y], accel[VEC3_Z], euler[VEC3_X]*RAD_TO_DEGREE,euler[VEC3_Y]*RAD_TO_DEGREE,euler[VEC3_Z]*RAD_TO_DEGREE, q0,q1,q2,q3, timedelay);
timeOld = time;
}
/*
* Mahony stream thread
*/
static msg_t stream_mahony_thread(void *arg) {
attitude_t attitude_data;
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
while (TRUE) {
float mx = ((float)mag_data.x - (-440.0)) / (510 - (-440)) * 2 - 1.0;
float my = ((float)mag_data.y - (-740.0)) / (380 - (-740)) * 2 - 1.0;
float mz = ((float)mag_data.z - (-500.0)) / (500 - (-500)) * 2 - 1.0;
MahonyAHRSupdate((-gyro_data.x / 57.143) * 3.141592 / 180.0,
(gyro_data.y / 57.143) * 3.141592 / 180.0,
-(gyro_data.z / 57.143) * 3.141592 / 180.0,
-acc_data.x / 1000.0, acc_data.y / 1000.0,
acc_data.z / 1000.0, mx, -my, -mz);
getMahAttitude(&attitude_data);
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f\r\n", (int)time, attitude_data.roll,
attitude_data.pitch, attitude_data.yaw);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
