Bootstrap Sketch for Yaw Pitch Rollerchair
/**************************************************************************\ | |
* Pinoccio Library * | |
* https://github.com/Pinoccio/library-pinoccio * | |
* Copyright (c) 2014, Pinoccio Inc. All rights reserved. * | |
* ------------------------------------------------------------------------ * | |
* This program is free software; you can redistribute it and/or modify it * | |
* under the terms of the MIT License as described in license.txt. * | |
\**************************************************************************/ | |
#include <SPI.h> | |
#include <Wire.h> | |
#include <Scout.h> | |
#include <GS.h> | |
#include <bitlash.h> | |
#include <lwm.h> | |
#include <js0n.h> | |
#include "version.h" | |
// IMU ----------------------------- | |
#include "I2Cdev.h" | |
#include "MPU6050_9Axis_MotionApps41.h" | |
// Declare device MPU6050 class | |
MPU6050 mpu; | |
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) | |
#define GyroMeasError PI * (40.0f / 180.0f) // gyroscope measurement error in rads/s (shown as 3 deg/s) | |
#define GyroMeasDrift PI * (0.0f / 180.0f) // gyroscope measurement drift in rad/s/s (shown as 0.0 deg/s/s) | |
#define beta sqrt(3.0f / 4.0f) * GyroMeasError // compute beta | |
#define zeta sqrt(3.0f / 4.0f) * GyroMeasDrift // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value | |
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral | |
#define Ki 0.0f | |
int16_t a1, a2, a3, g1, g2, g3, m1, m2, m3; // raw data arrays reading | |
uint16_t count = 0; // used to control display output rate | |
uint16_t delt_t = 0; // used to control display output rate | |
uint16_t mcount = 0; // used to control display output rate | |
uint8_t MagRate; // read rate for magnetometer data | |
float pitch, yaw, roll; | |
float deltat = 0.0f; // integration interval for both filter schemes | |
uint32_t lastUpdate = 0; // used to calculate integration interval | |
uint32_t now = 0; // used to calculate integration interval | |
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values | |
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion | |
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method | |
// Helper functions | |
static numvar mpuReport(void) { | |
char buf[64]; | |
sprintf(buf, "{\"yaw\":%i, \"pitch\":%i, \"roll\":%i}", (int)yaw, (int)pitch, (int)roll); | |
Shell.eval("hq.report", "mpu", buf); | |
speol(buf); | |
return 1; | |
} | |
void setup() { | |
Scout.setup(SKETCH_NAME, SKETCH_REVISION, SKETCH_BUILD); | |
Shell.addFunction("mpu.report", mpuReport); | |
// IMU --------------------------------- | |
// initialize MPU6050 device | |
// Serial.println(F("Initializing I2C devices...")); | |
mpu.initialize(); | |
// verify connection | |
// Serial.println(F("Testing device connections...")); | |
// Serial.println(mpu.testConnection() ? F("MPU9150 connection successful") : F("MPU9150 connection failed")); | |
// Set up the accelerometer, gyro, and magnetometer for data output | |
mpu.setRate(7); // set gyro rate to 8 kHz/(1 * rate) shows 1 kHz, accelerometer ODR is fixed at 1 KHz | |
MagRate = 10; // set magnetometer read rate in Hz; 10 to 100 (max) Hz are reasonable values | |
mpu.setDLPFMode(4); // set bandwidth of both gyro and accelerometer to ~20 Hz | |
// Full-scale range of the gyro sensors: | |
// 0 = +/- 250 degrees/sec, 1 = +/- 500 degrees/sec, 2 = +/- 1000 degrees/sec, 3 = +/- 2000 degrees/sec | |
mpu.setFullScaleGyroRange(0); // set gyro range to 250 degrees/sec | |
// Full-scale accelerometer range. | |
// The full-scale range of the accelerometer: 0 = +/- 2g, 1 = +/- 4g, 2 = +/- 8g, 3 = +/- 16g | |
mpu.setFullScaleAccelRange(0); // set accelerometer to 2 g range | |
mpu.setIntDataReadyEnabled(true); // enable data ready interrupt | |
} | |
void loop() { | |
Scout.loop(); | |
// IMU -------------------------------- | |
if (mpu.getIntDataReadyStatus() == 1) { // wait for data ready status register to update all data registers | |
mcount++; | |
// read the raw sensor data | |
mpu.getAcceleration ( &a1, &a2, &a3 ); | |
ax = a1 * 2.0f / 32768.0f; // 2 g full range for accelerometer | |
ay = a2 * 2.0f / 32768.0f; | |
az = a3 * 2.0f / 32768.0f; | |
mpu.getRotation ( &g1, &g2, &g3 ); | |
gx = g1 * 250.0f / 32768.0f; // 250 deg/s full range for gyroscope | |
gy = g2 * 250.0f / 32768.0f; | |
gz = g3 * 250.0f / 32768.0f; | |
if (mcount > 1000 / MagRate) { // this is a poor man's way of setting the magnetometer read rate (see below) | |
mpu.getMag ( &m1, &m2, &m3 ); | |
mx = m1 * 10.0f * 1229.0f / 4096.0f + 18.0f; // milliGauss (1229 microTesla per 2^12 bits, 10 mG per microTesla) | |
my = m2 * 10.0f * 1229.0f / 4096.0f + 70.0f; // apply calibration offsets in mG that correspond to your environment and magnetometer | |
mz = m3 * 10.0f * 1229.0f / 4096.0f + 270.0f; | |
mcount = 0; | |
} | |
} | |
now = micros(); | |
deltat = ((now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update | |
lastUpdate = now; | |
MadgwickQuaternionUpdate(ax, ay, az, gx * PI / 180.0f, gy * PI / 180.0f, gz * PI / 180.0f, my, mx, mz); | |
// Serial print and/or display at 0.5 s rate independent of data rates | |
// delt_t = millis() - count; | |
// if (delt_t > 1000) { // update LCD once per half-second independent of read rate | |
// Serial.print("ax = "); Serial.print((int)1000*ax); | |
// Serial.print(" ay = "); Serial.print((int)1000*ay); | |
// Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); | |
// Serial.print("gx = "); Serial.print( gx, 2); | |
// Serial.print(" gy = "); Serial.print( gy, 2); | |
// Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); | |
// Serial.print("mx = "); Serial.print( (int)mx ); | |
// Serial.print(" my = "); Serial.print( (int)my ); | |
// Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); | |
// Serial.print("q0 = "); Serial.print(q[0]); | |
// Serial.print(" qx = "); Serial.print(q[1]); | |
// Serial.print(" qy = "); Serial.print(q[2]); | |
// Serial.print(" qz = "); Serial.println(q[3]); | |
// yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); | |
// pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); | |
// roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); | |
// pitch *= 180.0f / PI; | |
// yaw *= 180.0f / PI - 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 | |
// roll *= 180.0f / PI; | |
// Serial.print("Yaw, Pitch, Roll: "); | |
// Serial.print(yaw, 2); | |
// Serial.print(", "); | |
// Serial.print(pitch, 2); | |
// Serial.print(", "); | |
// Serial.println(roll, 2); | |
// Serial.print("rate = "); Serial.print((float)1.0f/deltat, 2); Serial.println(" Hz"); | |
// count = millis(); | |
// } | |
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); | |
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); | |
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); | |
pitch *= 180.0f / PI; | |
yaw *= 180.0f / PI - 0.2575; // Declination at Saint Paul, MN is 0° 15' 27" on 2014-09-09 | |
roll *= 180.0f / PI; | |
} | |
// IMU ------------------------------------------ | |
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) | |
{ | |
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability | |
float norm; | |
float hx, hy, _2bx, _2bz; | |
float s1, s2, s3, s4; | |
float qDot1, qDot2, qDot3, qDot4; | |
// Auxiliary variables to avoid repeated arithmetic | |
float _2q1mx; | |
float _2q1my; | |
float _2q1mz; | |
float _2q2mx; | |
float _4bx; | |
float _4bz; | |
float _2q1 = 2.0f * q1; | |
float _2q2 = 2.0f * q2; | |
float _2q3 = 2.0f * q3; | |
float _2q4 = 2.0f * q4; | |
float _2q1q3 = 2.0f * q1 * q3; | |
float _2q3q4 = 2.0f * q3 * q4; | |
float q1q1 = q1 * q1; | |
float q1q2 = q1 * q2; | |
float q1q3 = q1 * q3; | |
float q1q4 = q1 * q4; | |
float q2q2 = q2 * q2; | |
float q2q3 = q2 * q3; | |
float q2q4 = q2 * q4; | |
float q3q3 = q3 * q3; | |
float q3q4 = q3 * q4; | |
float q4q4 = q4 * q4; | |
// Normalise accelerometer measurement | |
norm = sqrt(ax * ax + ay * ay + az * az); | |
if (norm == 0.0f) return; // handle NaN | |
norm = 1.0f / norm; | |
ax *= norm; | |
ay *= norm; | |
az *= norm; | |
// Normalise magnetometer measurement | |
norm = sqrt(mx * mx + my * my + mz * mz); | |
if (norm == 0.0f) return; // handle NaN | |
norm = 1.0f / norm; | |
mx *= norm; | |
my *= norm; | |
mz *= norm; | |
// Reference direction of Earth's magnetic field | |
_2q1mx = 2.0f * q1 * mx; | |
_2q1my = 2.0f * q1 * my; | |
_2q1mz = 2.0f * q1 * mz; | |
_2q2mx = 2.0f * q2 * mx; | |
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.0f - 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.0f - 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); | |
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude | |
norm = 1.0f / norm; | |
s1 *= norm; | |
s2 *= norm; | |
s3 *= norm; | |
s4 *= norm; | |
// Compute rate of change of quaternion | |
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; | |
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; | |
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; | |
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; | |
// Integrate to yield quaternion | |
q1 += qDot1 * deltat; | |
q2 += qDot2 * deltat; | |
q3 += qDot3 * deltat; | |
q4 += qDot4 * deltat; | |
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion | |
norm = 1.0f / norm; | |
q[0] = q1 * norm; | |
q[1] = q2 * norm; | |
q[2] = q3 * norm; | |
q[3] = q4 * norm; | |
} | |
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) | |
{ | |
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability | |
float norm; | |
float hx, hy, bx, bz; | |
float vx, vy, vz, wx, wy, wz; | |
float ex, ey, ez; | |
float pa, pb, pc; | |
// Auxiliary variables to avoid repeated arithmetic | |
float q1q1 = q1 * q1; | |
float q1q2 = q1 * q2; | |
float q1q3 = q1 * q3; | |
float q1q4 = q1 * q4; | |
float q2q2 = q2 * q2; | |
float q2q3 = q2 * q3; | |
float q2q4 = q2 * q4; | |
float q3q3 = q3 * q3; | |
float q3q4 = q3 * q4; | |
float q4q4 = q4 * q4; | |
// Normalise accelerometer measurement | |
norm = sqrt(ax * ax + ay * ay + az * az); | |
if (norm == 0.0f) return; // handle NaN | |
norm = 1.0f / norm; // use reciprocal for division | |
ax *= norm; | |
ay *= norm; | |
az *= norm; | |
// Normalise magnetometer measurement | |
norm = sqrt(mx * mx + my * my + mz * mz); | |
if (norm == 0.0f) return; // handle NaN | |
norm = 1.0f / norm; // use reciprocal for division | |
mx *= norm; | |
my *= norm; | |
mz *= norm; | |
// Reference direction of Earth's magnetic field | |
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); | |
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); | |
bx = sqrt((hx * hx) + (hy * hy)); | |
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); | |
// Estimated direction of gravity and magnetic field | |
vx = 2.0f * (q2q4 - q1q3); | |
vy = 2.0f * (q1q2 + q3q4); | |
vz = q1q1 - q2q2 - q3q3 + q4q4; | |
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); | |
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); | |
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); | |
// Error is cross product between estimated direction and measured direction of gravity | |
ex = (ay * vz - az * vy) + (my * wz - mz * wy); | |
ey = (az * vx - ax * vz) + (mz * wx - mx * wz); | |
ez = (ax * vy - ay * vx) + (mx * wy - my * wx); | |
if (Ki > 0.0f) | |
{ | |
eInt[0] += ex; // accumulate integral error | |
eInt[1] += ey; | |
eInt[2] += ez; | |
} | |
else | |
{ | |
eInt[0] = 0.0f; // prevent integral wind up | |
eInt[1] = 0.0f; | |
eInt[2] = 0.0f; | |
} | |
// Apply feedback terms | |
gx = gx + Kp * ex + Ki * eInt[0]; | |
gy = gy + Kp * ey + Ki * eInt[1]; | |
gz = gz + Kp * ez + Ki * eInt[2]; | |
// Integrate rate of change of quaternion | |
pa = q2; | |
pb = q3; | |
pc = q4; | |
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); | |
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); | |
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); | |
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); | |
// Normalise quaternion | |
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); | |
norm = 1.0f / norm; | |
q[0] = q1 * norm; | |
q[1] = q2 * norm; | |
q[2] = q3 * norm; | |
q[3] = q4 * norm; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment