Add reset yaw command to OC AHRS sensor

lib/OC/gyro.py

@@ -17,6 +17,8 @@ class GyroSensor(EV3UartSensor):
     QUATERNION_CONVERSION = 10000
     ACC_CONVERSION = 1000
 
+    RESET_YAW = "RESET-YAW"
+
     def __init__(self, port: int):
         """
         Initialize a new OC gyro sensor object at the given port.
@@ -31,6 +33,12 @@ 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):
+        """
+        Reset yaw angle while in MODE_EULER to zero
+        """
+        self.write_command(GyroSensor.RESET_YAW)
 
     def euler_angles_mag(self) -> int:
         """

oc_sensors/gyro/Core/Inc/oc_gyro.h

@@ -104,6 +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;
 	float Gyro_Time_Delta;
 	uint32_t Measure_Count;
 	uint32_t Gyro_Count;
@@ -146,6 +147,7 @@ typedef struct {
 
 /* Function prototypes */
 void oc_gyro_init(void);
+void oc_gyro_fusion_reset(void);
 void oc_gyro_fusion_step(sensor_mode sensor_state);
 uint8_t oc_gyro_choose_data(uint16_t* data_send, sensor_mode sensor_state);
 void oc_gyro_read_mag_data(void);

oc_sensors/gyro/Core/Inc/oc_sensor_modes.h

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

oc_sensors/gyro/Core/Src/FusionOffset.c

@@ -27,7 +27,7 @@
 /**
  * @brief Threshold in degrees per second.
  */
-#define THRESHOLD (3.0f)
+#define THRESHOLD (1.0f)
 
 //------------------------------------------------------------------------------
 // Functions

oc_sensors/gyro/Core/Src/main.c

@@ -151,7 +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;
+						}
 					}
 				}
 				rx_i = 0;
@@ -246,6 +248,7 @@ int main(void)
   {
 	GREEN_on();
 	RED_off();
+	oc_gyro_fusion_reset();
 	reset_communication();
 	// Initial state - send metadata and wait for ACK
 	while(sensor_state == S_MODE_INIT || sensor_state == S_MODE_WAIT_ACK) {

oc_sensors/gyro/Core/Src/oc_gyro.c

@@ -171,6 +171,10 @@ void oc_gyro_init() {
 	FusionAhrsInitialise(&ahrs);				/* NOTE: Changes with FS! */
 }
 
+void oc_gyro_fusion_reset(void) {
+	FusionAhrsReset(&ahrs);
+}
+
 void oc_gyro_fusion_step(sensor_mode sensor_state) {
 	FusionVector accelerometer = { .axis = {
 				.x = Get_Accel_Value_In_G(Data.ICM + 1),
@@ -271,6 +275,10 @@ 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);
+		}
 	}
 	/* Get quaternion */
 	const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);

oc_sensors/gyro/gyro.launch

@@ -40,7 +40,7 @@
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>