int main(int argc, char *argv[]) { srand(1237823); if (argc > 1) { instability_fix = atoi(argv[1]); } int i, c = 0; int max_iter = 20000000; for (i = 0; i < max_iter; i++) { float acc_x = acc_noise() + 2.5; float acc_y = acc_noise() + 2.5; float acc_z = acc_noise() + 9.15; MadgwickAHRSupdateIMU(0.0f, 0.0f, 0.0f, acc_x, acc_y, acc_z); float euler[3]; float quat[4] = {q0, q1, q2, q3}; quat_2_euler(quat, euler); if (++c == (max_iter / 1000)) { c = 0; printf("%f %f %f %f %f %f\n", rad2deg(euler[0]), rad2deg(euler[1]), rad2deg(euler[2]), acc_x, acc_y, acc_z); } } }
void madgwick_ahrs_update(q_t *q, v3_t g, v3_t a, v3_t m, float deltaT) { sample_freq = deltaT; MadgwickAHRSupdateIMU(g.x, g.y, g.z, a.x, a.y, a.z); q->w = q0; q->x = q1; q->y = q2; q->z = q3; }
void *regulator_thread(void *arg) { unsigned long TARGET_FPS = 512; unsigned long TARGET_TIME_BETWEEN_UPDATES = 1000000 / TARGET_FPS; unsigned long now = microtime(); unsigned long lastUpdateTime = microtime(); unsigned int lastSecondTime = (int) (now / 1000000); unsigned int currentSeccond; unsigned fps = TARGET_FPS; unsigned int updates = 0; float gx, gy, gz, ax, ay, az; while(running) { now = microtime(); mpu6050_read_gyroscope_float(&gx, &gy, &gz); mpu6050_read_accelerometer_float(&ax, &ay, &az); MadgwickAHRSupdateIMU(gx * M_PI / 180.0f, gy * M_PI / 180.0f, gz * M_PI / 180.0f, ax, ay, az); regulator_state.roll = atan2(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); regulator_state.pitch = -asin(2.0f * (q1 * q3 - q0 * q2)); regulator_state.yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); regulator_state.roll *= 180.0f / M_PI; regulator_state.pitch *= 180.0f / M_PI; regulator_state.yaw *= 180.0f / M_PI; lastUpdateTime = now; updates++; currentSeccond = (int) (now / 1000000); if(currentSeccond > lastSecondTime) { fps = updates; updates = 0; lastSecondTime = currentSeccond; } while(now - lastUpdateTime < TARGET_TIME_BETWEEN_UPDATES) { now = microtime(); } } }
msg_t madgwick_node(void *arg) { r2p::Node node("madgwick"); r2p::Publisher<r2p::TiltMsg> tilt_pub; attitude_t attitude_data; systime_t time; (void) arg; chRegSetThreadName("madgwick"); i2cStart(&I2C_DRIVER, &i2c1cfg); spiStart(&SPI_DRIVER, &spi1cfg); extStart(&EXTD1, &extcfg); gyroRun(&SPI_DRIVER, NORMALPRIO); accRun(&I2C_DRIVER, NORMALPRIO); // magRun(&I2C_DRIVER, NORMALPRIO); node.advertise(tilt_pub, "tilt"); time = chTimeNow(); for (;;) { MadgwickAHRSupdateIMU((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 / 980.0); getMadAttitude(&attitude_data); r2p::TiltMsg *msgp; if (tilt_pub.alloc(msgp)) { msgp->angle = (-attitude_data.roll * 57.29578) - 2.35; // basketbot offset tilt_pub.publish(*msgp); } time += MS2ST(20); chThdSleepUntil(time); } return CH_SUCCESS; }
/** * @brief Main IMU function that should be called all the time in the main while loop. It updates the values of the sensors, calculates the real values from the raw reading and does the Madgwick algorithm to calculate the Euler angles from the thes IMU readings when the data is ready. The dataReady flags are set in the interrupt.cpp file in the ISR(TWI_vect) when the data is ready. The time since the last update is also measured independently for each sensor to get the effective update rate of each sensor. This is done by dividing sum variable (which is the total time since the last sensor rate update) by the sumCount variable (which is the number of readings taken since the last sensor rate update). These variables are global and can be used anywhere to measure this update rate. These values must be put to 0 after the rate is taken. */ void IMU::updateImuAndMadgwick() { if(ak8963DataReady) { ak8963DataReady = false; ak8963.setRawMagneticField(currentRawMag); ak8963.calculateMag(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeAk8963 = ((Now - lastUpdateAk8963 + OCR0A*timer0OverflowCountAk8963)*8/(float)F_CPU); timer0OverflowCountAk8963 = 0; lastUpdateAk8963 = Now; sumAk8963 += deltaTimeAk8963; // sum for averaging filter update rate sumCountAk8963++; // Page 38 of PS-MPU-9255 gives the axes orientations // the x axis on the MPU9255 represents the forward direction. Since on our board the front is actually the y // direction, a 90 degree rotation clockwise has to be made. For this x = y and y = -x for all 3 sensors. // on top of this, to make sure the convention of positive pitch yaw roll is respected, the final x is multiplied by -1 // for all sensors (x,y,z) becomes (-y,-x,z) // for the magnetometer, an extra modification is required // Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer; // the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro! // so we interchange x and y and multiply the Z axis by -1. // the total change is (x,y,z) becomes (-x,-y,-z)) xMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldX(); yMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldY(); zMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldZ(); MadgwickAHRSupdate(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick, xRotationMadgwick, yRotationMadgwick, zRotationMadgwick, xMagneticFieldMadgwick, yMagneticFieldMadgwick, zMagneticFieldMadgwick); calcEulerAngles(); } else if(bmp180DataReady) { bmp180DataReady = false; bmp180.setRawTemperature(currentRawTemp); bmp180.setRawPressure(currentRawPress); bmp180.calculateTrueValues(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeBmp180 = ((Now - lastUpdateBmp180 + OCR0A*timer0OverflowCountBmp180)*8/(float)F_CPU); timer0OverflowCountBmp180 = 0; lastUpdateBmp180 = Now; sumBmp180 += deltaTimeBmp180; // sum for averaging filter update rate sumCountBmp180++; } else if(mpu9255DataReady) { mpu9255DataReady = false; //These functions updates the values of the class variables with the FSM buffer variables mpu9255.setRawAcceleration(currentRawAcc); mpu9255.setRawRotation(currentRawGyr); mpu9255.calculateAccRotTemp(); //calculate the time it takes before the next call of this function Now = TCNT0; //the 8 is counter 0 prescaler deltaTimeMpu9255 = ((Now - lastUpdateMpu9255 + OCR0A*timer0OverflowCountMpu9255)*8/(float)F_CPU); timer0OverflowCountMpu9255 = 0; lastUpdateMpu9255 = Now; sumMpu9255 += deltaTimeMpu9255; // sum for averaging filter update rate sumCountMpu9255++; xAccelerationMadgwick = -(float)mpu9255.getAccelerationY()/10000; yAccelerationMadgwick = -(float)mpu9255.getAccelerationX()/10000; zAccelerationMadgwick = (float)mpu9255.getAccelerationZ()/10000; xRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800; yRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800; zRotationMadgwick = (float)mpu9255.getRotationZ()*M_PI_OVER_1800; MadgwickAHRSupdateIMU(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick, xRotationMadgwick, yRotationMadgwick, zRotationMadgwick); calcEulerAngles(); } }
/*----------------------------------------------------------------------------- * Service MAVlink protocol message routine */ void Timer1_IRQHandler() { mavlink_message_t msg; MadgwickAHRSupdateIMU( DEG_TO_RAD(params[PARAM_GX].val), DEG_TO_RAD(params[PARAM_GY].val), DEG_TO_RAD(params[PARAM_GZ].val), params[PARAM_AX].val, params[PARAM_AY].val, params[PARAM_AZ].val); mavlink_msg_heartbeat_pack( mavlink_system.sysid, mavlink_system.compid, &msg, MAV_TYPE_GROUND_ROVER, MAV_AUTOPILOT_GENERIC, mavlink_sys_mode, 0, mavlink_sys_state); mavlink_send_msg(&msg); mavlink_msg_sys_status_pack( mavlink_system.sysid, mavlink_system.compid, &msg, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_GPS, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG, MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_GPS, 50, // TODO remove hardoded values 0, -1, -1, 0, 0, 0, 0, 0, 0); mavlink_send_msg(&msg); mavlink_msg_autopilot_version_pack( mavlink_system.sysid, mavlink_system.compid, &msg, 0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD 42, 42, 42, 42, "DEADBEEF", "DEADBEEF", "DEADBEEF", 42, 42, 42); mavlink_send_msg(&msg); mavlink_msg_highres_imu_pack( mavlink_system.sysid, mavlink_system.compid, &msg, usec(), params[PARAM_AX].val, params[PARAM_AY].val, params[PARAM_AZ].val, params[PARAM_GX].val, params[PARAM_GY].val, params[PARAM_GZ].val, params[PARAM_MX].val, params[PARAM_MY].val, params[PARAM_MZ].val, 0, 0, 0, params[PARAM_T].val, (1 << 12) | ((1 << 9) - 1)); mavlink_send_msg(&msg); mavlink_msg_attitude_quaternion_pack( mavlink_system.sysid, mavlink_system.compid, &msg, usec(), q0, q1, q2, q3, DEG_TO_RAD(params[PARAM_GX].val), DEG_TO_RAD(params[PARAM_GY].val), DEG_TO_RAD(params[PARAM_GZ].val)); mavlink_send_msg(&msg); usec_service_routine(); MSS_TIM1_clear_irq(); }
QQuaternion& GradDes3DOrientation::MadgwickAHRSupdate(double ax, double ay, double az, double gx, double gy, double gz, double mx, double my, double mz) { double recipNorm; double s1, s2, s3, s4; double qDot1, qDot2, qDot3, qDot4; double hx, hy; double _2q1mx, _2q1my, _2q1mz, _2q2mx, _2bx, _2bz, _4bx, _4bz, _2q1, _2q2, _2q3, _2q4, _2q1q3, _2q3q4, q1q1, q1q2, q1q3, q1q4, q2q2, q2q3, q2q4, q3q3, q3q4, q4q4; q1 = quat.scalar(); q2 = quat.x(); q3 = quat.y(); q4 = quat.z(); // Use IMU algorithm if magnetometer measurement is invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { return MadgwickAHRSupdateIMU(ax, ay, az, gx, gy, gz); } // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz); qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy); qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx); qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx); // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = 1.0 / qSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = 1.0 / qSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic _2q1mx = 2.0 * q1 * mx; _2q1my = 2.0 * q1 * my; _2q1mz = 2.0 * q1 * mz; _2q2mx = 2.0 * q2 * mx; _2q1 = 2.0 * q1; _2q2 = 2.0 * q2; _2q3 = 2.0 * q3; _2q4 = 2.0 * q4; _2q1q3 = 2.0 * q1 * q3; _2q3q4 = 2.0 * q3 * q4; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q1q4 = q1 * q4; q2q2 = q2 * q2; q2q3 = q2 * q3; q2q4 = q2 * q4; q3q3 = q3 * q3; q3q4 = q3 * q4; q4q4 = q4 * q4; // Reference direction of Earth's magnetic field hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; _2bx = sqrt(hx * hx + hy * hy); _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; // Gradient decent algorithm corrective step s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1 - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1 - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); recipNorm = 1.0 / qSqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; s4 *= recipNorm; // Apply feedback step qDot1 -= mBeta * s1; qDot2 -= mBeta * s2; qDot3 -= mBeta * s3; qDot4 -= mBeta * s4; } // Integrate rate of change of quaternion to yield quaternion quat += QQuaternion(qDot1*mSamplingPeriod, qDot2*mSamplingPeriod, qDot3*mSamplingPeriod, qDot4*mSamplingPeriod); quat.normalize(); return quat; }
//--------------------------------------------------------------------------------------------------- // AHRS algorithm update void MadgwickAHRS::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; float hx, hy; float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); return; } // Rate of change of quaternion from gyroscope qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic _2q0mx = 2.0f * q0 * mx; _2q0my = 2.0f * q0 * my; _2q0mz = 2.0f * q0 * mz; _2q1mx = 2.0f * q1 * mx; _2q0 = 2.0f * q0; _2q1 = 2.0f * q1; _2q2 = 2.0f * q2; _2q3 = 2.0f * q3; _2q0q2 = 2.0f * q0 * q2; _2q2q3 = 2.0f * q2 * q3; q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; // Reference direction of Earth's magnetic field hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; _2bx = sqrt(hx * hx + hy * hy); _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; _4bx = 2.0f * _2bx; _4bz = 2.0f * _2bz; // Gradient decent algorithm corrective step s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude s0 *= recipNorm; s1 *= recipNorm; s2 *= recipNorm; s3 *= recipNorm; // Apply feedback step qDot1 -= beta * s0; qDot2 -= beta * s1; qDot3 -= beta * s2; qDot4 -= beta * s3; } // Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * (1.0f / sampleFreq); q1 += qDot2 * (1.0f / sampleFreq); q2 += qDot3 * (1.0f / sampleFreq); q3 += qDot4 * (1.0f / sampleFreq); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; }
void MainWindow::processSensorsData(TUdpDataSENSORS* data) { int32_t time = data->ticks; /*m_pdaData.frustum[0]*=10; m_pdaData.frustum[1]*=10; m_pdaData.frustum[2]*=10; m_pdaData.frustum[3]*=10;*/ m_pdaData.sphereEnabled = data->sphereEnabled; m_pdaData.ax = -data->ax; m_pdaData.ay = -data->ay; m_pdaData.az = -data->az; m_pdaData.gx = data->gx; m_pdaData.gy = data->gy; m_pdaData.gz = data->gz; m_pdaData.mx = -data->mx; m_pdaData.my = -data->my; m_pdaData.mz = -data->mz; m_pdaData.worldQuat = QQuaternion(data->q0, data->q1, data->q2, data->q3); if (m_pdaData.mx < minX) minX = m_pdaData.mx; if (m_pdaData.my < minY) minY = m_pdaData.my; if (m_pdaData.mz < minZ) minZ = m_pdaData.mz; if (m_pdaData.mx > maxX) maxX = m_pdaData.mx; if (m_pdaData.my > maxY) maxY = m_pdaData.my; if (m_pdaData.mz > maxZ) maxZ = m_pdaData.mz; //-66.75 -46.75 -40.25 // 40.75 40.5 40.5 static int64_t last = 0; if (last == 0) last = time; int64_t diff = time - last; if (diff > 1000) { //qDebug() << "RES"; m_accelQuat = QQuaternion(1, 0, 0, 0); m_gyroQuat = QQuaternion(1, 0, 0, 0); m_gyroAccelQuat = QQuaternion(1, 0, 0, 0); m_accelMagnetQuat = QQuaternion(1, 0, 0, 0); m_fullQuat = QQuaternion(1, 0, 0, 0); last = 0; diff = 100; return; } last = time; //qDebug() << "DIFF" << time << diff; MadgwickAHRSupdateIMU(m_accelQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f * 5); MadgwickAHRSupdateIMU(m_gyroQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, 0, 0, 0, (float)diff / 1000.0f); MadgwickAHRSupdateIMU(m_gyroAccelQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f); MadgwickAHRSupdate(m_accelMagnetQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f); MadgwickAHRSupdate(m_fullQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f); ui->visAccel->rotationQuat = m_accelQuat; ui->visGyro->rotationQuat = m_gyroQuat; ui->visGyroAccel->rotationQuat = m_gyroAccelQuat; ui->visAccelMagnet->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90); ui->visAccelMagnet->rotationQuat *= m_accelMagnetQuat; ui->visFull->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90); ui->visFull->rotationQuat *= m_fullQuat; ui->visWorld->rotationQuat = m_pdaData.worldQuat.conjugate(); ui->visWorld2->rotationQuat = m_pdaData.worldQuat.conjugate(); ui->visScreen->rotationQuat = m_pdaData.worldQuat.conjugate(); //static float q = 0; //ui->visWorld->rotationQuat *= QQuaternion::fromAxisAndAngle(0, 1, 1, q += 1); float qq0 = m_fullQuat.scalar(); float qq1 = m_fullQuat.x(); float qq2 = m_fullQuat.y(); float qq3 = m_fullQuat.z(); float q2_2 = qq2*qq2; m_pdaData.pitch = atanf (2.0f*(qq0*qq1 + qq2*qq3) / (1.0f - 2.0f*(qq1*qq1 + q2_2))); m_pdaData.roll = asinf (2.0f*(qq0*qq2 - qq3*qq1)); m_pdaData.yaw = atan2f (2.0f*(qq0*qq3 + qq1*qq2), (1.0f - 2.0f*(q2_2 + qq3*qq3))); ui->rawReads->updateInfo(m_pdaData); }
int main(void) { int scope1dt,scope1t,scope1s,scope1d,scope1j; int scope2dt,scope2t,scope2s,scope2d,scope2j; char znakr; char znak; GPIO_InitTypeDef GPIO_str; uint8_t ii; uint8_t i = 0; /* SysTick end of count event each 0,01ms 0,00001*/// 0,01ms -100000 RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100000); /* Accelerometer Configuration */ Acc_Config(); Demo_GyroConfig(); ii = SystemCoreClock; /* This is a way to read the System core clock */ CONF_TIMERS(); CONF_PWMIN(); confI2C(); while(1) I2C_start(); /////////// Przyciski RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); GPIO_str.GPIO_Pin= GPIO_Pin_4 | GPIO_Pin_5; GPIO_str.GPIO_Mode=GPIO_Mode_IN; GPIO_str.GPIO_PuPd=GPIO_PuPd_UP; GPIO_str.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_str); ii = 0; USART2_Init(115200); GPIO_SetBits(GPIOD, GPIO_Pin_0); LEFT =3400; RIGHT=3400; FRONT=3400; REAR=3400; //delay_ms(4000); while (1) { esf=SystemCoreClock/360/(TIM2->CCR2); esd = (TIM2->CCR1*100);///(TIM2->CCR2); throttle=(int)(esd*32.0/156.0-1236.0); esf3=SystemCoreClock/360/(TIM3->CCR2); if(throttle>6600) throttle=3400; esd3 = (TIM3->CCR1*100);///(TIM3->CCR2); pitch_zadany=(float)(-6.0*esd3/1800.0+100.0)+0.0; if(pitch_zadany>40 || pitch_zadany<-40) pitch_zadany=0; esd4 = (TIM4->CCR1*100); roll_zadany=(float)(-6.0*esd4/1800.0+100.0); if(roll_zadany>40 || roll_zadany<-40) roll_zadany=0; //esd5 =(TIM8->CCR2*100)/TIM8->CCR1; //yaw_zadany=(float)(-6.0*esd5/1800.0+100.0); //if(GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5)==0) //flaga=0; //IMU/////////////////////////////////////////////////////// Acc_ReadData(AccBuffer); for(i=0;i<3;i++) AccBuffer[i] /= 100.0f; acc_x=AccBuffer[0]; acc_y=AccBuffer[1]; acc_z=AccBuffer[2]; Demo_GyroReadAngRate(Buffer); gyr_x=Buffer[0]; gyr_y=Buffer[1]; gyr_z=Buffer[2]; MadgwickAHRSupdateIMU( -gyr_y*0.01745, gyr_x*0.01745, gyr_z*0.01745, acc_x, acc_y, acc_z); roll=180/PI*atan2(2*(q2*q3+q0*q1),1-2*(q1*q1+q2*q2));//(q0*q0-q1*q1-q2*q2+q3*q3)); pitch=180/PI*asin(-2*(q1*q3-q0*q2)); yaw=180/PI*atan2(2*(q1*q2+q0*q3),(1-2*(q3*q3+q2*q2))); scope1=PIDY*1000;//niebieski scope2=yaw*1000;//czerwony if(scope1>0) znak='+'; else znak='-'; if(scope2>0) znakr='+'; else znakr='-'; scope1=abs(scope1); scope2=abs(scope2); CZAS_S=(float)CZAS/100000.0; CZAS=0; scope1dt=scope1/10000; scope1t=scope1/1000-scope1dt*10; scope1s=scope1/100-scope1dt*100-scope1t*10; scope1d=scope1/10-scope1dt*1000-scope1t*100-scope1s*10; scope1j=scope1-scope1dt*10000-scope1t*1000-scope1s*100-scope1d*10; scope2dt=scope2/10000; scope2t=scope2/1000-scope2dt*10; scope2s=scope2/100-scope2dt*100-scope2t*10; scope2d=scope2/10-scope2dt*1000-scope2t*100-scope2s*10; scope2j=scope2-scope2dt*10000-scope2t*1000-scope2s*100-scope2d*10; printf("%c%d%d%d%d%d %c%d%d%d%d%d\r\n",znak,scope1dt,scope1t,scope1s,scope1d,scope1j,znakr,scope2dt,scope2t,scope2s,scope2d,scope2j); /*PIDY=-PIDyawrate(yaw_zadany, yaw); rollratezadane=PIDroll(roll_zadany, roll); PIDR=PIDrollrate(rollratezadane, -gyr_y); speedLeft=(int)((float)throttle+PIDR-PIDY);//throttle+PIDR; speedRight=(int)((float)throttle-PIDR-PIDY); pitchratezadane=PIDpitch(pitch_zadany, pitch); PIDP=PIDpitchrate(pitchratezadane, gyr_x);//float PIDpitchrollrate(float zadana, float aktualna) speedFront=(int)((float)throttle-PIDP+PIDY); speedRear=(int)((float)throttle+PIDP+PIDY); */ if(speedFront>speedmax) speedFront=speedmax; else if(speedFront<speedmin) speedFront=speedmin; else ; if(speedRear>speedmax) speedRear=speedmax; else if(speedRear<speedmin) speedRear=speedmin; else ; if(speedLeft>speedmax) speedLeft=speedmax; else if(speedLeft<speedmin) speedLeft=speedmin; else ; if(speedRight>speedmax) speedRight=speedmax; else if(speedRight<speedmin) speedRight=speedmin; else ; if(throttle>3450) { LEFT =(int)speedLeft; RIGHT=(int)speedRight; FRONT=3400;//(int)speedFront; REAR=3400;//(int)speedRear; } else { LEFT =3400; RIGHT=3400; FRONT=3400; REAR=3400; } } return 0; }
void process_new_sensor_values(float gyro_x, float gyro_y, float gyro_z, float accel_x, float accel_y, float accel_z, float magn_x, float magn_y, float magn_z) { // sensor fusion with Madgwick's Filter // MadgwickAHRSupdate(gyro_z, gyro_y, -gyro_x, accel_z, accel_y, -accel_x, magn_z, magn_y, -magn_x); MadgwickAHRSupdateIMU(gyro_z, gyro_y, -gyro_x, accel_z, accel_y, -accel_x); // calculate the pitch angle so that: 0 = vertical -pi/2 = on its back +pi/2 = on its face float pitch = asinf(-2.0f * (q1*q3 - q0*q2)); // calculate the set point (desired angle) and error (difference between the current angle and desired angle) // since there are no wheel encoders, only throttle affects the set point // mapping throttle to an angle so that: 0 = no throttle -pi/10 = full speed reverse +pi/10 = full speed forward float set_point = (float) gimbalY / 1400.0f * 0.314159265f; float error = pitch - set_point; // calculate the proportional component (current error * p scalar) float p_scalar = 12000.0f + (knobLeft - 2048.0f) * 5.90f; if(p_scalar < 0) p_scalar = 0; float proportional = error * p_scalar; // calculate the integral component (summation of past errors * i scalar) float i_scalar = 500.0f + (knobMiddle - 2048.0f) * 0.27f; if(i_scalar < 0) i_scalar = 0; static float integral = 0; integral += error * i_scalar; if(integral > 1000) integral = 1000; // limit wind-up if(integral < -1000) integral = -1000; // calculate the derivative component (change since previous error * d scalar) static float previous_error = 0; float d_scalar = 16000.0f + (knobRight - 2048.0f) * 7.85f; if(d_scalar < 0) d_scalar = 0; float derivative = (error - previous_error) * d_scalar; previous_error = error; int32_t motor_a_speed = proportional + integral + derivative; int32_t motor_b_speed = proportional + integral + derivative; // apply steering motor_a_speed += gimbalX / 2; motor_b_speed -= gimbalX / 2; // stop the motors if we're far from vertical since there is no chance of success if(pitch < -0.7f || pitch > 0.7f) { motor_a_speed = 0; motor_b_speed = 0; } timer_dual_hbridge_motor_speeds(motor_a_speed, motor_b_speed); uart_send_bin_floats(27, accel_x, // G accel_y, // G accel_z, // G gyro_x, // Rad/s gyro_y, // Rad/s gyro_z, // Rad/s magn_x, // Gs magn_y, // Gs magn_z, // Gs pitch, // Rad q0, // Quaternion q1, // Quaternion q2, // Quaternion q3, // Quaternion gimbalX, gimbalY, knobLeft, knobMiddle, knobRight, set_point, error, p_scalar, proportional, i_scalar, integral, d_scalar, derivative); }