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);
}
