104 lines
2.3 KiB
C
104 lines
2.3 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." : "IMU calibration failed.";
|
|
jsonInfoHttp["r"] = icm_roll;
|
|
jsonInfoHttp["p"] = icm_pitch;
|
|
jsonInfoHttp["y"] = icm_yaw;
|
|
|
|
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["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;
|
|
|
|
String getInfoJsonString;
|
|
serializeJson(jsonInfoHttp, getInfoJsonString);
|
|
Serial.println(getInfoJsonString);
|
|
}
|
|
|
|
void setIMUOffset(int16_t inputX, int16_t inputY, int16_t inputZ) {
|
|
imuSetMagnOffsets(inputX, inputY, inputZ);
|
|
getIMUOffset();
|
|
}
|