Change OC AHRS reset yaw to reset all angles

lib/OC/gyro.py

@@ -17,7 +17,7 @@ class GyroSensor(EV3UartSensor):
     QUATERNION_CONVERSION = 10000
     ACC_CONVERSION = 1000
 
-    RESET_YAW = "RESET-YAW"
+    RESET_ANGLES = "RESET-ANGLES"
 
     def __init__(self, port: int):
         """
@@ -34,11 +34,11 @@ class GyroSensor(EV3UartSensor):
         values = self.read_raw_mode(GyroSensor.MODE_EULER)
         return (values[0]/self.EULER_CONVERSION, values[1]/self.EULER_CONVERSION, values[2]/self.EULER_CONVERSION)
     
-    def reset_yaw(self):
+    def reset_angles(self):
         """
-        Reset yaw angle while in MODE_EULER to zero
+        Reset angles to zero while in EULER, EULER_MAG or QUATERNION mode
         """
-        self.write_command(GyroSensor.RESET_YAW)
+        self.write_command(GyroSensor.RESET_ANGLES)
 
     def euler_angles_mag(self) -> int:
         """

oc_sensors/gyro/Core/Inc/oc_gyro.h

@@ -104,7 +104,7 @@ typedef struct {
 	uint8_t Gyro_Timer_Running : 1;
 	uint8_t Gyro_Calibrated : 1;
 	uint8_t Mag_Calibrated : 1;
-	uint8_t Reset_Yaw : 1;
+	uint8_t Reset_Angles : 1;
 	float Gyro_Time_Delta;
 	uint32_t Measure_Count;
 	uint32_t Gyro_Count;

oc_sensors/gyro/Core/Inc/oc_sensor_modes.h

@@ -6,8 +6,8 @@
 
 #define SENSOR_ID 195
 #define DATA_BAUD_RATE 256000
-#define RESET_YAW_STRING "RESET-YAW"
-#define RESET_YAW_SIZE 9
+#define RESET_YAW_STRING "RESET-ANGLES"
+#define RESET_YAW_SIZE 12
 
 typedef enum {
 	S_MODE_WAIT_ACK = -2,

oc_sensors/gyro/Core/Src/FusionAhrs.c

@@ -18,12 +18,12 @@
 /**
  * @brief Initial gain used during the initialisation.
  */
-#define INITIAL_GAIN (10.0f)
+#define INITIAL_GAIN (15.0f)
 
 /**
  * @brief Initialisation period in seconds.
  */
-#define INITIALISATION_PERIOD (3.0f)
+#define INITIALISATION_PERIOD (0.5f)
 
 //------------------------------------------------------------------------------
 // Function declarations

oc_sensors/gyro/Core/Src/main.c

@@ -151,8 +151,9 @@ void USART2_Rec_Cal(void) {
 							Status.Should_Send_UART = 1;
 						}
 					} else if (msg_subtype == MTYPE_COMMAND_WRITE) {
-						if ((sensor_state == S_MODE_EULER) && (strncmp(&rx_buff[1], RESET_YAW_STRING, RESET_YAW_SIZE) == 0)) {
-							Status.Reset_Yaw = 1;
+						if ((sensor_state == S_MODE_EULER || sensor_state == S_MODE_EULER_MAG || sensor_state == S_MODE_QUATERNION)
+								&& (strncmp(&rx_buff[1], RESET_YAW_STRING, RESET_YAW_SIZE) == 0)) {
+							Status.Reset_Angles = 1;
 						}
 					}
 				}

oc_sensors/gyro/Core/Src/oc_gyro.c

@@ -49,6 +49,7 @@ oc_gyro_calibration HardIronCal = {
 
 FusionVector GyroCalSum = {0};
 FusionVector GyroCal = {0};
+FusionQuaternion ResetQuat = FUSION_IDENTITY_QUATERNION;
 
 static void Write_To_SPI1_Sync(uint8_t reg_addr, uint8_t* p_write,
 	const uint8_t bytes_to_write);
@@ -173,6 +174,7 @@ void oc_gyro_init() {
 
 void oc_gyro_fusion_reset(void) {
 	FusionAhrsReset(&ahrs);
+	ResetQuat = FUSION_IDENTITY_QUATERNION;
 }
 
 void oc_gyro_fusion_step(sensor_mode sensor_state) {
@@ -275,13 +277,19 @@ void oc_gyro_fusion_step(sensor_mode sensor_state) {
 	else {
 		FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer,
 				Status.Gyro_Time_Delta);
-		if (Status.Reset_Yaw) {
-			Status.Reset_Yaw = 0;
-			FusionAhrsSetHeading(&ahrs, 0.0);
-		}
+	}
+	if (Status.Reset_Angles) {
+		Status.Reset_Angles = 0;
+		ResetQuat.array[0] = ahrs.quaternion.array[0];
+		ResetQuat.array[1] = -ahrs.quaternion.array[1];
+		ResetQuat.array[2] = -ahrs.quaternion.array[2];
+		ResetQuat.array[3] = -ahrs.quaternion.array[3];
+
 	}
 	/* Get quaternion */
-	const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
+	FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
+	q = FusionQuaternionMultiply(q, ResetQuat);
+	FusionQuaternionNormalise(q);
 	const FusionEuler euler = FusionQuaternionToEuler(q);
 	latest_acc = accelerometer;
 	latest_mag = magnetometer;