wip loop time fix

.cproject

@@ -49,7 +49,7 @@
 
                             <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1391827141" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.4 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G474RETx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Include | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc ||  ||  || USE_HAL_DRIVER | STM32G474xx ||  || Drivers | Core/Startup | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32G474RETX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None" valueType="string"/>
 
-                            <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1518244944" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
+                            <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.1518244944" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="true" valueType="boolean"/>
 
                             <targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.954385215" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 
@@ -67,7 +67,7 @@
 
                                 <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.1344316198" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.debuglevel.value.g3" valueType="enumerated"/>
 
-                                <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1641270532" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false"/>
+                                <option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1641270532" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" useByScannerDiscovery="false" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.ofast" valueType="enumerated"/>
 
                                 <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.2084931892" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 
@@ -339,6 +339,20 @@
 
     </storageModule>
 
-    <storageModule moduleId="refreshScope"/>
+    <storageModule moduleId="refreshScope" versionNumber="2">
+
+        <configuration configurationName="Debug">
+
+            <resource resourceType="PROJECT" workspacePath="/motorcontrol_stm32g474re"/>
+
+        </configuration>
+
+        <configuration configurationName="Release">
+
+            <resource resourceType="PROJECT" workspacePath="/motorcontrol_stm32g474re"/>
+
+        </configuration>
+
+    </storageModule>
 
 </cproject>

.settings/org.eclipse.cdt.core.prefs

@@ -0,0 +1,6 @@
+doxygen/doxygen_new_line_after_brief=true
+doxygen/doxygen_use_brief_tag=false
+doxygen/doxygen_use_javadoc_tags=true
+doxygen/doxygen_use_pre_tag=false
+doxygen/doxygen_use_structural_commands=false
+eclipse.preferences.version=1

Core/Inc/calibration.h

@@ -14,7 +14,7 @@
 #define V_CAL 	1.0f							// Calibration voltage
 //#define I_CAL	5.0f
 //#define W_CAL 	10.0f							// Calibration speed in rad/s
-#define W_CAL 2.0f
+#define W_CAL 10.0f
 #define T1		1.0f							// Cal settling period
 #define PPAIRS_MAX 64
 #define SAMPLES_PER_PPAIR N_LUT

Core/Inc/hw_config.h

@@ -57,18 +57,22 @@
 #define DTC_MIN 			0.0f          	// Min duty cycle
 #define DTC_COMP 			0.000f          // deadtime compensation (100 ns / 25 us)
 // original #define DT					.000025f		// Loop period
-#define DT					.000029f		// Loop period
+#define DT					.000060f		// Loop period
 #define EN_ENC_LINEARIZATION 1				// Enable/disable encoder linearization
 
 
 /* Current controller */
 #define L_D .000003f				// D axis inductance
 #define L_Q .000003f				// Q axis inductance
-#define K_D .05f                    // Loop gain,  Volts/Amp
-#define K_Q .05f                    // Loop gain,  Volts/Amp
+//#define K_D .05f //orig                   // Loop gain,  Volts/Amp
+//#define K_Q .05f //orig                   // Loop gain,  Volts/Amp
+#define K_D .05f
+#define K_Q .05fc
 #define K_SCALE 0.0001f             // K_loop/Loop BW (Hz) 0.0042
-#define KI_D 0.045f                // PI zero, in radians per sample
-#define KI_Q 0.045f                // PI zero, in radians per sample
+//#define KI_D 0.045f //orig               // PI zero, in radians per sample
+//#define KI_Q 0.045f //orig               // PI zero, in radians per sample
+#define KI_D 0.045f
+#define KI_Q 0.045f
 #define OVERMODULATION 1.15f        // 1.0 = no overmodulation
 #define CURRENT_FILT_ALPHA	.01f	// 1st order d/q current filter (not used in control)
 #define VBUS_FILT_ALPHA		.1f		// 1st order bus voltage filter

Core/Src/calibration.c

@@ -21,11 +21,15 @@ void order_phases(EncoderStruct *encoder, ControllerStruct *controller, CalStruc
 
 	static unsigned debugCounter = 0;
 
+	static int pf1, pf2, pf3, pf4, pf5;
+
+
 
 	if(!cal->started){
 		printf("Checking phase sign, pole pairs\r\n");
 		cal->started = 1;
 		cal->start_count = loop_count;
+		pf1 = pf2 = pf3 = pf4 = pf5 = 1;
 	}
 	cal->time = (float)(loop_count - cal->start_count)*DT;
 
@@ -77,6 +81,10 @@ void order_phases(EncoderStruct *encoder, ControllerStruct *controller, CalStruc
 		}
 
     if(cal->time < T1){
+    	if(pf1){
+    		printf("p1\n\r");
+    		pf1 = 0;
+    	}
 	    // Set voltage angle to zero, wait for rotor position to settle
         cal->theta_ref = 0;//W_CAL*cal->time;
         cal->cal_position.elec_angle = cal->theta_ref;
@@ -88,6 +96,10 @@ void order_phases(EncoderStruct *encoder, ControllerStruct *controller, CalStruc
     	return;
     }
     else if(cal->time < T1+2.0f*PI_F/W_CAL){
+    	if(pf2){
+    	    printf("p2\n\r");
+    	    pf2 = 0;
+    	}
     	// rotate voltage vector through one electrical cycle
     	cal->theta_ref = W_CAL*(cal->time-T1);
     	cal->cal_position.elec_angle = cal->theta_ref;

Core/Src/fsm.c

@@ -67,7 +67,8 @@
 
 		 case MOTOR_MODE:
 			 /* If CAN has timed out, reset all commands */
-			 if((CAN_TIMEOUT > 0 ) && (controller.timeout > CAN_TIMEOUT)){
+			 #define just_stay_alive_forever_please 1 //todo remove this
+			 if((!just_stay_alive_forever_please) && (CAN_TIMEOUT > 0 ) && (controller.timeout > CAN_TIMEOUT)){
 				 zero_commands(&controller);
 			 }
 			 /* Otherwise, commutate */

Core/Src/main.c

@@ -191,8 +191,6 @@ int main(void)
 
   }
 
-  printf("erase %d ", erase_reserved_flash());
-
   init_controller_params(&controller);
 
   /* calibration "encoder" zeroing */
@@ -313,7 +311,7 @@ int main(void)
   /* USER CODE BEGIN WHILE */
   while (1)
   {
-	  HAL_Delay(150);
+	  HAL_Delay(100);
 
 	  if(! DRV_DISABLED ) drv_print_faults(drv);
 
@@ -323,14 +321,14 @@ int main(void)
 
 
 	  //printf("ppairs %f \n\r", PPAIRS);
-	  //printf("vbus %f \n\r",controller.v_bus);
+	  //printf("vbus %f \n\r",controller.v_bus);`
 	  //printf("Ria,b %d %d \n\r", controller.adc_a_raw, controller.adc_b_raw);
 	  //printf("ia,b %f %f | %d %d \n\r", controller.i_a, controller.i_b, controller.adc_a_raw-controller.adc_a_offset, controller.adc_b_raw-controller.adc_b_offset);
 	  //pack_reply(&can_tx, CAN_ID,  0.0, 0.0, 0.0);	// Pack response
 	  //HAL_FDCAN_AddMessageToTxFifoQ(&CAN_H, &can_tx.tx_header, can_tx.data); //replacement for above line
 	  //printf("sent\n");
 	  if(state.state==MOTOR_MODE){
-	  //	  printf("%.2f %.2f %.2f %.2f %.2f\r\n", controller.i_a, controller.i_b, controller.i_d, controller.i_q, controller.dtheta_elec);
+	  	  printf("%.2f %.2f %.2f %.2f %.2f %.2f\r\n", controller.i_a, controller.i_b, controller.i_d, controller.i_q, controller.dtheta_elec, controller.dtheta_mech);
 	  }
     /* USER CODE END WHILE */
 

Core/Src/position_sensor.c

@@ -46,7 +46,6 @@ void ps_sample(EncoderStruct * encoder, float dt){
 	int off_interp = off_1 + ((off_2 - off_1)*(encoder->raw - ((encoder->raw>>9)<<9))>>9);     // Interpolate between lookup table entries
 	encoder->count = encoder->raw + off_interp;
 
-
 	/* Real angles in radians */
 	encoder->angle_singleturn = ((float)(encoder->count-M_ZERO))/((float)ENC_CPR);
 	int int_angle = encoder->angle_singleturn;

Core/Src/tim.c

@@ -45,7 +45,7 @@ void MX_TIM1_Init(void)
   htim1.Instance = TIM1;
   htim1.Init.Prescaler = 0;
   htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
-  htim1.Init.Period = 4700;
+  htim1.Init.Period = 4250; //4700 - 27.8u
   htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim1.Init.RepetitionCounter = 0;
   htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;

Debug/Core/Src/adc.su

@@ -1,5 +1,5 @@
-adc.c:32:6:MX_ADC1_Init	56	static
-adc.c:92:6:MX_ADC2_Init	40	static
-adc.c:144:6:MX_ADC3_Init	56	static
+adc.c:32:6:MX_ADC1_Init	64	static
+adc.c:92:6:MX_ADC2_Init	48	static
+adc.c:144:6:MX_ADC3_Init	64	static
 adc.c:206:6:HAL_ADC_MspInit	64	static
-adc.c:281:6:HAL_ADC_MspDeInit	16	static
+adc.c:281:6:HAL_ADC_MspDeInit	0	static

Debug/Core/Src/calibration.su

@@ -1,3 +1,3 @@
-calibration.c:17:6:order_phases	96	static
-calibration.c:119:6:calibrate_encoder	96	static
-calibration.c:220:6:measure_lr	24	static
+calibration.c:17:6:order_phases	56	static
+calibration.c:131:6:calibrate_encoder	40	static
+calibration.c:232:6:measure_lr	0	static

Debug/Core/Src/drv8353.su

@@ -1,14 +1,14 @@
 drv8353.c:14:10:drv_spi_write	24	static
-drv8353.c:22:10:drv_read_FSR1	16	static
-drv8353.c:26:10:drv_read_FSR2	16	static
+drv8353.c:22:10:drv_read_FSR1	24	static
+drv8353.c:26:10:drv_read_FSR2	24	static
 drv8353.c:30:10:drv_read_register	24	static
-drv8353.c:33:6:drv_write_register	32	static
-drv8353.c:37:6:drv_write_DCR	40	static
-drv8353.c:44:6:drv_write_HSR	40	static
-drv8353.c:49:6:drv_write_LSR	40	static
-drv8353.c:54:6:drv_write_OCPCR	40	static
-drv8353.c:59:6:drv_write_CSACR	40	static
-drv8353.c:64:6:drv_enable_gd	24	static
-drv8353.c:69:6:drv_disable_gd	24	static
-drv8353.c:74:6:drv_calibrate	24	static
-drv8353.c:79:6:drv_print_faults	24	static
+drv8353.c:33:6:drv_write_register	24	static
+drv8353.c:37:6:drv_write_DCR	32	static
+drv8353.c:44:6:drv_write_HSR	24	static
+drv8353.c:49:6:drv_write_LSR	32	static
+drv8353.c:54:6:drv_write_OCPCR	32	static
+drv8353.c:59:6:drv_write_CSACR	32	static
+drv8353.c:64:6:drv_enable_gd	40	static
+drv8353.c:69:6:drv_disable_gd	40	static
+drv8353.c:74:6:drv_calibrate	32	static
+drv8353.c:79:6:drv_print_faults	40	static

Debug/Core/Src/fdcan.su

@@ -1,7 +1,7 @@
 fdcan.c:35:6:MX_FDCAN2_Init	8	static
 fdcan.c:72:6:HAL_FDCAN_MspInit	48	static
-fdcan.c:105:6:HAL_FDCAN_MspDeInit	16	static
+fdcan.c:105:6:HAL_FDCAN_MspDeInit	8	static
 fdcan.c:131:6:can_rx_init	24	static
-fdcan.c:161:6:can_tx_init	16	static
-fdcan.c:194:6:pack_reply	48	static
+fdcan.c:161:6:can_tx_init	0	static
+fdcan.c:194:6:pack_reply	32	static
 fdcan.c:222:6:unpack_cmd	40	static

Debug/Core/Src/flash_access.su

@@ -1,3 +1,3 @@
-flash_access.c:38:6:load_from_flash	80	static
-flash_access.c:73:5:erase_reserved_flash	32	static
-flash_access.c:92:5:save_to_flash	40	static
+flash_access.c:38:6:load_from_flash	4	static
+flash_access.c:73:5:erase_reserved_flash	40	static
+flash_access.c:92:5:save_to_flash	48	static

Debug/Core/Src/foc.su

@@ -1,15 +1,15 @@
-foc.c:19:6:set_dtc	32	static
+foc.c:19:6:set_dtc	0	static
 foc.c:49:6:analog_sample	16	static
-foc.c:88:6:abc	40	static
+foc.c:88:6:abc	32	static
 foc.c:102:6:dq0	40	static
 foc.c:115:6:svm	56	static
-foc.c:128:6:zero_current	32	static
-foc.c:149:6:init_controller_params	24	static
-foc.c:165:6:reset_foc	16	static
-foc.c:184:6:reset_observer	16	static
-foc.c:192:6:update_observer	16	static
-foc.c:224:7:linearize_dtc	32	static
-foc.c:233:6:field_weaken	24	static
-foc.c:245:6:commutate	40	static
-foc.c:301:6:torque_control	24	static
-foc.c:311:6:zero_commands	16	static
+foc.c:128:6:zero_current	40	static
+foc.c:149:6:init_controller_params	40	static
+foc.c:165:6:reset_foc	0	static
+foc.c:184:6:reset_observer	0	static
+foc.c:192:6:update_observer	0	static
+foc.c:224:7:linearize_dtc	8	static
+foc.c:233:6:field_weaken	16	static
+foc.c:245:6:commutate	48	static
+foc.c:301:6:torque_control	0	static
+foc.c:311:6:zero_commands	0	static

Debug/Core/Src/fsm.su

@@ -1,8 +1,8 @@
+fsm.c:135:7:fsm_exit_state	16	static
+fsm.c:319:7:enter_menu_state	8	static
+fsm.c:337:7:enter_setup_state	56	static
+fsm.c:96:7:fsm_enter_state	8	static
 fsm.c:22:7:run_fsm	24	static
-fsm.c:95:7:fsm_enter_state	16	static
-fsm.c:134:7:fsm_exit_state	16	static
-fsm.c:174:7:update_fsm	32	static
-fsm.c:318:7:enter_menu_state	8	static
-fsm.c:336:7:enter_setup_state	24	static
-fsm.c:361:7:process_user_input	16	static
-fsm.c:450:7:enter_motor_mode	4	static
+fsm.c:362:7:process_user_input	16	static
+fsm.c:175:7:update_fsm	24	static
+fsm.c:451:7:enter_motor_mode	0	static

Debug/Core/Src/gpio.su

@@ -1 +1 @@
-gpio.c:41:6:MX_GPIO_Init	48	static
+gpio.c:41:6:MX_GPIO_Init	64	static

Debug/Core/Src/main.su

@@ -1,4 +1,3 @@
-core_cm4.h:1809:22:__NVIC_SetPriority	16	static
-main.c:122:5:main	40	static
-main.c:346:6:SystemClock_Config	168	static
-main.c:406:6:Error_Handler	4	static
+main.c:344:6:SystemClock_Config	192	static
+main.c:122:5:main	56	static
+main.c:404:6:Error_Handler	0	static

Debug/Core/Src/math_ops.su

@@ -1,10 +1,10 @@
-math_ops.c:6:7:fast_fmaxf	16	static
-math_ops.c:11:7:fast_fminf	16	static
-math_ops.c:16:7:fmaxf3	24	static
-math_ops.c:21:7:fminf3	24	static
-math_ops.c:31:6:limit_norm	32	static
-math_ops.c:40:6:limit	24	static
-math_ops.c:44:5:float_to_uint	32	static
-math_ops.c:52:7:uint_to_float	32	static
+math_ops.c:6:7:fast_fmaxf	0	static
+math_ops.c:11:7:fast_fminf	0	static
+math_ops.c:16:7:fmaxf3	0	static
+math_ops.c:21:7:fminf3	0	static
+math_ops.c:31:6:limit_norm	0	static
+math_ops.c:40:6:limit	0	static
+math_ops.c:44:5:float_to_uint	0	static
+math_ops.c:52:7:uint_to_float	0	static
 math_ops.c:59:7:sin_lut	16	static
 math_ops.c:66:7:cos_lut	16	static

Debug/Core/Src/position_sensor.su

@@ -1,3 +1,3 @@
 position_sensor.c:14:6:ps_warmup	32	static
-position_sensor.c:25:6:ps_sample	48	static
-position_sensor.c:95:6:ps_print	16	static
+position_sensor.c:25:6:ps_sample	32	static
+position_sensor.c:94:6:ps_print	8	static

Debug/Core/Src/spi.su

@@ -1,4 +1,4 @@
-spi.c:31:6:MX_SPI1_Init	8	static
+spi.c:31:6:MX_SPI1_Init	16	static
 spi.c:65:6:MX_SPI3_Init	8	static
 spi.c:99:6:HAL_SPI_MspInit	56	static
-spi.c:155:6:HAL_SPI_MspDeInit	16	static
+spi.c:155:6:HAL_SPI_MspDeInit	0	static

Debug/Core/Src/stm32g4xx_hal_msp.su

@@ -1 +1 @@
-stm32g4xx_hal_msp.c:64:6:HAL_MspInit	16	static
+stm32g4xx_hal_msp.c:64:6:HAL_MspInit	8	static

Debug/Core/Src/stm32g4xx_it.su

@@ -1,12 +1,12 @@
-stm32g4xx_it.c:84:6:NMI_Handler	4	static
-stm32g4xx_it.c:99:6:HardFault_Handler	4	static
-stm32g4xx_it.c:114:6:MemManage_Handler	4	static
-stm32g4xx_it.c:129:6:BusFault_Handler	4	static
-stm32g4xx_it.c:144:6:UsageFault_Handler	4	static
-stm32g4xx_it.c:159:6:SVC_Handler	4	static
-stm32g4xx_it.c:172:6:DebugMon_Handler	4	static
-stm32g4xx_it.c:185:6:PendSV_Handler	4	static
-stm32g4xx_it.c:198:6:SysTick_Handler	8	static
-stm32g4xx_it.c:219:6:TIM1_UP_TIM16_IRQHandler	8	static
-stm32g4xx_it.c:246:6:USART2_IRQHandler	16	static
+stm32g4xx_it.c:84:6:NMI_Handler	0	static
+stm32g4xx_it.c:99:6:HardFault_Handler	0	static
+stm32g4xx_it.c:114:6:MemManage_Handler	0	static
+stm32g4xx_it.c:129:6:BusFault_Handler	0	static
+stm32g4xx_it.c:144:6:UsageFault_Handler	0	static
+stm32g4xx_it.c:159:6:SVC_Handler	0	static
+stm32g4xx_it.c:172:6:DebugMon_Handler	0	static
+stm32g4xx_it.c:185:6:PendSV_Handler	0	static
+stm32g4xx_it.c:198:6:SysTick_Handler	0	static
+stm32g4xx_it.c:219:6:TIM1_UP_TIM16_IRQHandler	16	static
+stm32g4xx_it.c:246:6:USART2_IRQHandler	8	static
 stm32g4xx_it.c:261:6:FDCAN2_IT0_IRQHandler	88	static