Initial import of WAVE_ROVER_V1.0

2026-04-23 17:52:46 +02:00
commit bdd2b4315b
28 changed files with 8449 additions and 0 deletions
@@ -0,0 +1,103 @@
+#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();
+}