Initial import of WAVE_ROVER_V1.0
This commit is contained in:
Joshua Sacherer
@@ -0,0 +1,359 @@
|
||||
#include "IMU.h"
|
||||
|
||||
void calibrateMagn();
|
||||
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
|
||||
float invSqrt(float x);
|
||||
|
||||
/******************************************************************************
|
||||
* IMU module *
|
||||
******************************************************************************/
|
||||
// #define S_SCL 33
|
||||
// #define S_SDA 32
|
||||
|
||||
AK09918_err_type_t err;
|
||||
|
||||
QMI8658 qmi8658_;
|
||||
AK09918 magnetometer_;
|
||||
|
||||
int16_t offset_x = -12, offset_y = 0, offset_z = 0;
|
||||
int16_t x, y, z;
|
||||
// Find the magnetic declination at your location
|
||||
// http://www.magnetic-declination.com/
|
||||
double declination_shenzhen = -3.22;
|
||||
|
||||
#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
|
||||
#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases
|
||||
|
||||
float angles[3];
|
||||
float q0, q1, q2, q3;
|
||||
float exInt = 0.0f;
|
||||
float eyInt = 0.0f;
|
||||
float ezInt = 0.0f;
|
||||
uint32_t lastFilterUpdateUs = 0;
|
||||
|
||||
namespace {
|
||||
constexpr float kDegToRad = 0.01745329251994329577f;
|
||||
constexpr float kDefaultSampleDt = 0.01f;
|
||||
constexpr float kMaxSampleDt = 0.1f;
|
||||
|
||||
float clampUnit(float value)
|
||||
{
|
||||
if (value > 1.0f) {
|
||||
return 1.0f;
|
||||
}
|
||||
if (value < -1.0f) {
|
||||
return -1.0f;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
void resetFilterState()
|
||||
{
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
exInt = 0.0f;
|
||||
eyInt = 0.0f;
|
||||
ezInt = 0.0f;
|
||||
lastFilterUpdateUs = micros();
|
||||
}
|
||||
|
||||
float getSampleDeltaSeconds()
|
||||
{
|
||||
const uint32_t nowUs = micros();
|
||||
|
||||
if (lastFilterUpdateUs == 0) {
|
||||
lastFilterUpdateUs = nowUs;
|
||||
return kDefaultSampleDt;
|
||||
}
|
||||
|
||||
const uint32_t elapsedUs = nowUs - lastFilterUpdateUs;
|
||||
lastFilterUpdateUs = nowUs;
|
||||
|
||||
const float deltaSeconds = elapsedUs * 1.0e-6f;
|
||||
if (deltaSeconds <= 0.0f || deltaSeconds > kMaxSampleDt) {
|
||||
return kDefaultSampleDt;
|
||||
}
|
||||
|
||||
return deltaSeconds;
|
||||
}
|
||||
|
||||
bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
|
||||
{
|
||||
const float normSquared = (*xAxis * *xAxis) + (*yAxis * *yAxis) + (*zAxis * *zAxis);
|
||||
if (normSquared <= 1.0e-12f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float norm = invSqrt(normSquared);
|
||||
*xAxis *= norm;
|
||||
*yAxis *= norm;
|
||||
*zAxis *= norm;
|
||||
return true;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void imuInit()
|
||||
{
|
||||
// Wire.begin(S_SDA, S_SCL);
|
||||
// Serial.begin(115200);
|
||||
|
||||
if (qmi8658_.begin() == 0)
|
||||
Serial.println("qmi8658_init fail");
|
||||
|
||||
if (magnetometer_.initialize())
|
||||
Serial.println("AK09918_init fail") ;
|
||||
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
|
||||
err = magnetometer_.isDataReady();
|
||||
int retry_times = 0;
|
||||
while (err != AK09918_ERR_OK) {
|
||||
Serial.println(err);
|
||||
Serial.println("Waiting Sensor");
|
||||
delay(100);
|
||||
magnetometer_.reset();
|
||||
delay(100);
|
||||
magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
|
||||
err = magnetometer_.isDataReady();
|
||||
retry_times ++;
|
||||
if (retry_times > 10) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
// Serial.println("Start figure-8 calibration after 1 seconds.");
|
||||
// delay(1000);
|
||||
// calibrate(10000, &offset_x, &offset_y, &offset_z);
|
||||
// calibrateMagn();
|
||||
resetFilterState();
|
||||
}
|
||||
|
||||
void imuDataGet(EulerAngles *pstAngles,
|
||||
IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
|
||||
IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
|
||||
IMU_ST_SENSOR_DATA *pstMagnRawData)
|
||||
{
|
||||
if (pstAngles == nullptr || pstGyroRawData == nullptr ||
|
||||
pstAccelRawData == nullptr || pstMagnRawData == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
float acc[3] = {0.0f, 0.0f, 0.0f};
|
||||
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||
float MotionVal[9];
|
||||
|
||||
const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
|
||||
if (magErr == AK09918_ERR_OVERFLOW) {
|
||||
Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
|
||||
}
|
||||
|
||||
pstMagnRawData->s16X = x- offset_x;
|
||||
pstMagnRawData->s16Y = y- offset_y;
|
||||
pstMagnRawData->s16Z = z- offset_z;
|
||||
|
||||
// qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
|
||||
qmi8658_.read_sensor_data(acc,gyro);
|
||||
|
||||
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]);
|
||||
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2])));
|
||||
|
||||
// double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
|
||||
// double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
|
||||
|
||||
// pstAngles->yaw = /*180 + */57.3 * atan2(Yheading, Xheading) + declination_shenzhen;
|
||||
|
||||
// pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
|
||||
// pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
|
||||
MotionVal[0]=gyro[0];
|
||||
MotionVal[1]=gyro[1];
|
||||
MotionVal[2]=gyro[2];
|
||||
MotionVal[3]=acc[0];
|
||||
MotionVal[4]=acc[1];
|
||||
MotionVal[5]=acc[2];
|
||||
MotionVal[6]=pstMagnRawData->s16X;
|
||||
MotionVal[7]=pstMagnRawData->s16Y;
|
||||
MotionVal[8]=pstMagnRawData->s16Z;
|
||||
|
||||
imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
|
||||
(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
|
||||
(float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
|
||||
|
||||
pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
|
||||
pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
|
||||
-2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
|
||||
pstAngles->yaw = atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3,
|
||||
2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * 57.2957795f;
|
||||
|
||||
pstGyroRawData->X = gyro[0];
|
||||
pstGyroRawData->Y = gyro[1];
|
||||
pstGyroRawData->Z = gyro[2];
|
||||
|
||||
pstAccelRawData->X = acc[0];
|
||||
pstAccelRawData->Y = acc[1];
|
||||
pstAccelRawData->Z = acc[2];
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
bool imuRecalibrate()
|
||||
{
|
||||
qmi8658_.autoOffsets();
|
||||
resetFilterState();
|
||||
return true;
|
||||
}
|
||||
|
||||
void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset)
|
||||
{
|
||||
if (pstMagnOffset == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
pstMagnOffset->s16X = offset_x;
|
||||
pstMagnOffset->s16Y = offset_y;
|
||||
pstMagnOffset->s16Z = offset_z;
|
||||
}
|
||||
|
||||
void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
|
||||
{
|
||||
offset_x = inputX;
|
||||
offset_y = inputY;
|
||||
offset_z = inputZ;
|
||||
resetFilterState();
|
||||
}
|
||||
|
||||
float imuGetTemperature()
|
||||
{
|
||||
return qmi8658_.read_temperature();
|
||||
}
|
||||
|
||||
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
|
||||
{
|
||||
const float sampleDt = getSampleDeltaSeconds();
|
||||
const float halfT = 0.5f * sampleDt;
|
||||
float hx = 0.0f, hy = 0.0f, hz = 0.0f, bx = 0.0f, bz = 0.0f;
|
||||
float vx = 0.0f, vy = 0.0f, vz = 0.0f, wx = 0.0f, wy = 0.0f, wz = 0.0f;
|
||||
float ex = 0.0f, ey = 0.0f, ez = 0.0f;
|
||||
|
||||
float q0q0 = q0 * q0;
|
||||
float q0q1 = q0 * q1;
|
||||
float q0q2 = q0 * q2;
|
||||
float q0q3 = q0 * q3;
|
||||
float q1q1 = q1 * q1;
|
||||
float q1q2 = q1 * q2;
|
||||
float q1q3 = q1 * q3;
|
||||
float q2q2 = q2 * q2;
|
||||
float q2q3 = q2 * q3;
|
||||
float q3q3 = q3 * q3;
|
||||
|
||||
const bool accelValid = normalizeVector(&ax, &ay, &az);
|
||||
const bool magnValid = normalizeVector(&mx, &my, &mz);
|
||||
|
||||
if (accelValid) {
|
||||
vx = 2.0f * (q1q3 - q0q2);
|
||||
vy = 2.0f * (q0q1 + q2q3);
|
||||
vz = q0q0 - q1q1 - q2q2 + q3q3;
|
||||
|
||||
ex += (ay * vz - az * vy);
|
||||
ey += (az * vx - ax * vz);
|
||||
ez += (ax * vy - ay * vx);
|
||||
}
|
||||
|
||||
if (magnValid) {
|
||||
hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
|
||||
hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
|
||||
hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
|
||||
bx = sqrtf((hx * hx) + (hy * hy));
|
||||
bz = hz;
|
||||
|
||||
wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
|
||||
wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
|
||||
wz = 2.0f * bx * (q0q2 + q1q3) + 2.0f * bz * (0.5f - q1q1 - q2q2);
|
||||
|
||||
ex += (my * wz - mz * wy);
|
||||
ey += (mz * wx - mx * wz);
|
||||
ez += (mx * wy - my * wx);
|
||||
}
|
||||
|
||||
if (accelValid || magnValid) {
|
||||
exInt += ex * Ki * sampleDt;
|
||||
eyInt += ey * Ki * sampleDt;
|
||||
ezInt += ez * Ki * sampleDt;
|
||||
|
||||
gx = gx + Kp * ex + exInt;
|
||||
gy = gy + Kp * ey + eyInt;
|
||||
gz = gz + Kp * ez + ezInt;
|
||||
}
|
||||
|
||||
const float q0Prev = q0;
|
||||
const float q1Prev = q1;
|
||||
const float q2Prev = q2;
|
||||
const float q3Prev = q3;
|
||||
|
||||
q0 = q0Prev + (-q1Prev * gx - q2Prev * gy - q3Prev * gz) * halfT;
|
||||
q1 = q1Prev + (q0Prev * gx + q2Prev * gz - q3Prev * gy) * halfT;
|
||||
q2 = q2Prev + (q0Prev * gy - q1Prev * gz + q3Prev * gx) * halfT;
|
||||
q3 = q3Prev + (q0Prev * gz + q1Prev * gy - q2Prev * gx) * halfT;
|
||||
|
||||
const float normSquared = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
|
||||
if (normSquared <= 1.0e-12f) {
|
||||
resetFilterState();
|
||||
return;
|
||||
}
|
||||
|
||||
const float norm = invSqrt(normSquared);
|
||||
q0 = q0 * norm;
|
||||
q1 = q1 * norm;
|
||||
q2 = q2 * norm;
|
||||
q3 = q3 * norm;
|
||||
}
|
||||
|
||||
float invSqrt(float x)
|
||||
{
|
||||
if (x <= 0.0f) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float halfx = 0.5f * x;
|
||||
union {
|
||||
float f;
|
||||
uint32_t i;
|
||||
} conv = { x };
|
||||
|
||||
conv.i = 0x5f3759df - (conv.i >> 1);
|
||||
|
||||
float y = conv.f;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
void calibrateMagn(void)
|
||||
{
|
||||
int16_t temp[9];
|
||||
Serial.printf("keep 10dof-imu device horizontal and it will read x y z axis offset value after 4 seconds\n");
|
||||
delay(4000);
|
||||
Serial.printf("start read all axises offset value\n");
|
||||
magnetometer_.getData(&x, &y, &z);
|
||||
temp[0] = x;
|
||||
temp[1] = y;
|
||||
temp[2] = z;
|
||||
|
||||
Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
|
||||
delay(4000);
|
||||
Serial.printf("start read all axises offset value\n");
|
||||
magnetometer_.getData(&x, &y, &z);
|
||||
temp[3] = x;
|
||||
temp[4] = y;
|
||||
temp[5] = z;
|
||||
|
||||
Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
|
||||
delay(4000);
|
||||
Serial.printf("start read all axises offset value\n");
|
||||
magnetometer_.getData(&x, &y, &z);
|
||||
temp[6] = x;
|
||||
temp[7] = y;
|
||||
temp[8] = z;
|
||||
|
||||
offset_x = (temp[0]+temp[3])/2;
|
||||
offset_y = (temp[1]+temp[4])/2;
|
||||
offset_z = (temp[5]+temp[8])/2;
|
||||
}
|
||||
Reference in New Issue
Block a user