131 lines
3.7 KiB
C
131 lines
3.7 KiB
C
#include"IMU.h"
|
|
|
|
// define GPIOs for IIC.
|
|
EulerAngles stAngles;
|
|
IMU_ST_SENSOR_DATA_FLOAT stGyroRawData;
|
|
IMU_ST_SENSOR_DATA_FLOAT stAccelRawData;
|
|
IMU_ST_SENSOR_DATA stMagnRawData;
|
|
float temp;
|
|
|
|
|
|
void imu_init() {
|
|
imuInit();
|
|
}
|
|
|
|
|
|
void updateIMUData() {
|
|
imuDataGet( &stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);
|
|
temp = imuGetTemperature();
|
|
|
|
ax = stAccelRawData.X;
|
|
ay = stAccelRawData.Y;
|
|
az = stAccelRawData.Z;
|
|
|
|
mx = stMagnRawData.s16X;
|
|
my = stMagnRawData.s16Y;
|
|
mz = stMagnRawData.s16Z;
|
|
|
|
gx = stGyroRawData.X;
|
|
gy = stGyroRawData.Y;
|
|
gz = stGyroRawData.Z;
|
|
|
|
icm_roll = stAngles.roll;
|
|
icm_pitch = stAngles.pitch;
|
|
icm_yaw = stAngles.yaw;
|
|
icm_temp = temp;
|
|
last_imu_update = millis();
|
|
}
|
|
|
|
|
|
void imuCalibration() {
|
|
const bool calibrationOk = imuRecalibrate();
|
|
updateIMUData();
|
|
|
|
jsonInfoHttp.clear();
|
|
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
|
jsonInfoHttp["status"] = calibrationOk ? 1 : 0;
|
|
jsonInfoHttp["info"] = calibrationOk ? "IMU calibration finished. Rotate the rover slowly once to refine heading." : "IMU calibration failed.";
|
|
jsonInfoHttp["r"] = icm_roll;
|
|
jsonInfoHttp["p"] = icm_pitch;
|
|
jsonInfoHttp["y"] = icm_yaw;
|
|
jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
|
|
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
|
|
|
|
String getInfoJsonString;
|
|
serializeJson(jsonInfoHttp, getInfoJsonString);
|
|
Serial.println(getInfoJsonString);
|
|
}
|
|
|
|
void startMagCalibration() {
|
|
const bool started = imuStartMagnCalibration();
|
|
|
|
jsonInfoHttp.clear();
|
|
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
|
jsonInfoHttp["status"] = started ? 1 : 0;
|
|
jsonInfoHttp["info"] = started ? "Mag calibration started. Rotate the rover slowly through different angles until progress reaches 100." : "Mag calibration failed to start.";
|
|
jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
|
|
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
|
|
|
|
String getInfoJsonString;
|
|
serializeJson(jsonInfoHttp, getInfoJsonString);
|
|
Serial.println(getInfoJsonString);
|
|
}
|
|
|
|
|
|
void getIMUData() {
|
|
jsonInfoHttp.clear();
|
|
jsonInfoHttp["T"] = FEEDBACK_IMU_DATA;
|
|
|
|
jsonInfoHttp["r"] = icm_roll;
|
|
jsonInfoHttp["p"] = icm_pitch;
|
|
jsonInfoHttp["y"] = icm_yaw;
|
|
|
|
jsonInfoHttp["ax"] = ax;
|
|
jsonInfoHttp["ay"] = ay;
|
|
jsonInfoHttp["az"] = az;
|
|
|
|
jsonInfoHttp["gx"] = gx;
|
|
jsonInfoHttp["gy"] = gy;
|
|
jsonInfoHttp["gz"] = gz;
|
|
|
|
jsonInfoHttp["mx"] = mx;
|
|
jsonInfoHttp["my"] = my;
|
|
jsonInfoHttp["mz"] = mz;
|
|
jsonInfoHttp["magCal"] = imuHasHeadingCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magSaved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
|
jsonInfoHttp["magCalRunning"] = imuIsMagnCalibrationRunning() ? 1 : 0;
|
|
jsonInfoHttp["magCalProgress"] = imuGetMagnCalibrationProgress();
|
|
|
|
jsonInfoHttp["temp"] = temp;
|
|
|
|
String getInfoJsonString;
|
|
serializeJson(jsonInfoHttp, getInfoJsonString);
|
|
Serial.println(getInfoJsonString);
|
|
}
|
|
|
|
void getIMUOffset() {
|
|
IMU_ST_SENSOR_DATA offsetData;
|
|
imuGetMagnOffsets(&offsetData);
|
|
|
|
jsonInfoHttp.clear();
|
|
jsonInfoHttp["T"] = CMD_GET_IMU_OFFSET;
|
|
jsonInfoHttp["x"] = offsetData.s16X;
|
|
jsonInfoHttp["y"] = offsetData.s16Y;
|
|
jsonInfoHttp["z"] = offsetData.s16Z;
|
|
jsonInfoHttp["saved"] = imuHasStoredMagnCalibration() ? 1 : 0;
|
|
|
|
String getInfoJsonString;
|
|
serializeJson(jsonInfoHttp, getInfoJsonString);
|
|
Serial.println(getInfoJsonString);
|
|
}
|
|
|
|
void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
|
|
imuSetMagnOffsets(inputX, inputY, inputZ);
|
|
imuSaveMagnCalibration();
|
|
getIMUOffset();
|
|
}
|