Optimization updates, added pwm-driver and added pwm extensions

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_adc.c

@@ -100,7 +100,10 @@
   * limitations under the License.
   *
   ******************************************************************************
-  */ 
+  */
+
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
 
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_adc.h"
@@ -1726,7 +1729,10 @@ void ADC_ClearITPendingBit(ADC_TypeDef* ADCx, uint16_t ADC_IT)
   itmask = (uint8_t)(ADC_IT >> 8);
   /* Clear the selected ADC interrupt pending bits */
   ADCx->SR = ~(uint32_t)itmask;
-}                    
+}
+
+#pragma GCC pop_options
+
 /**
   * @}
   */ 

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_dma.c

@@ -120,6 +120,9 @@
   ******************************************************************************  
   */ 
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_dma.h"
 #include "stm32f4xx_rcc.h"
@@ -1282,6 +1285,8 @@ void DMA_ClearITPendingBit(DMA_Stream_TypeDef* DMAy_Streamx, uint32_t DMA_IT)
   }   
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_exti.c

@@ -64,6 +64,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_exti.h"
 
@@ -293,6 +296,8 @@ void EXTI_ClearITPendingBit(uint32_t EXTI_Line)
   EXTI->PR = EXTI_Line;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_flash.c

@@ -68,6 +68,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_flash.h"
 
@@ -1608,6 +1611,8 @@ FLASH_Status FLASH_WaitForLastOperation(void)
   return status;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */ 

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_iwdg.c

@@ -81,6 +81,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_iwdg.h"
 
@@ -247,6 +250,8 @@ FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG)
   return bitstatus;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_rcc.c

@@ -55,6 +55,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_rcc.h"
 
@@ -2839,6 +2842,8 @@ void RCC_ClearITPendingBit(uint8_t RCC_IT)
   *(__IO uint8_t *) CIR_BYTE3_ADDRESS = RCC_IT;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */ 

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_syscfg.c

@@ -46,6 +46,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_syscfg.h"
 #include "stm32f4xx_rcc.h"
@@ -244,6 +247,9 @@ void SYSCFG_BreakConfig(uint32_t SYSCFG_Break)
   SYSCFG->CFGR2 |= (uint32_t) SYSCFG_Break;
 }
 #endif /* STM32F410xx */
+
+#pragma GCC pop_options
+
 /**
   * @}
   */

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_tim.c

@@ -115,6 +115,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_tim.h"
 #include "stm32f4xx_rcc.h"
@@ -3350,6 +3353,8 @@ static void TI4_Config(TIM_TypeDef* TIMx, uint16_t TIM_ICPolarity, uint16_t TIM_
   TIMx->CCER = tmpccer ;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */

ChibiOS_3.0.5/ext/stdperiph_stm32f4/src/stm32f4xx_wwdg.c

@@ -80,6 +80,9 @@
   ******************************************************************************
   */
 
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_wwdg.h"
 #include "stm32f4xx_rcc.h"
@@ -288,6 +291,8 @@ void WWDG_ClearFlag(void)
   WWDG->SR = (uint32_t)RESET;
 }
 
+#pragma GCC pop_options
+
 /**
   * @}
   */

applications/app.c

@@ -29,7 +29,7 @@
 #include "comm_can.h"
 #include "imu.h"
 #include "crc.h"
-#include "servo_simple.h"
+#include "pwm_servo.h"
 #include "servo_dec.h"
 
 // Private variables
@@ -87,9 +87,9 @@ void app_set_configuration(app_configuration *conf) {
 				appconf.app_to_use != APP_PPM_UART &&
 				appconf.servo_out_enable) {
 			servodec_stop();
-			servo_simple_init();
+			pwm_servo_init_servo();
 		} else {
-			servo_simple_stop();
+			pwm_servo_stop();
 		}
 
 		switch (appconf.app_to_use) {

comm/commands.c

@@ -25,7 +25,7 @@
 #include "hal.h"
 #include "mc_interface.h"
 #include "stm32f4xx_conf.h"
-#include "servo_simple.h"
+#include "pwm_servo.h"
 #include "buffer.h"
 #include "terminal.h"
 #include "hw.h"
@@ -534,7 +534,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 
 	case COMM_SET_SERVO_POS: {
 		int32_t ind = 0;
-		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
+		pwm_servo_set_servo_out(buffer_get_float16(data, 1000.0, &ind));
 	} break;
 
 	case COMM_SET_MCCONF: {

driver/driver.mk

@@ -3,9 +3,9 @@ CSRC += \
 	driver/i2c_bb.c \
 	driver/ledpwm.c \
 	driver/servo_dec.c \
-	driver/servo_simple.c \
 	driver/spi_bb.c \
-	driver/timer.c
+	driver/timer.c \
+	driver/pwm_servo.c
 
 CSRC += \
 	driver/lora/SX1278.c \

driver/pwm_servo.c

@@ -0,0 +1,124 @@
+/*
+	Copyright 2024 Benjamin Vedder	[email protected]
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+#include "pwm_servo.h"
+#include "ch.h"
+#include "hal.h"
+#include "conf_general.h"
+#include "utils.h"
+
+#pragma GCC push_options
+#pragma GCC optimize ("Os")
+
+// Settings
+#define TIM_CLOCK			2000000 // Hz
+
+// Private variables
+static volatile bool m_is_running = false;
+
+uint32_t pwm_servo_init(uint32_t freq_hz) {
+	// Ensure that there is no overflow and that the resolution is reasonable
+	utils_truncate_number_uint32(&freq_hz, TIM_CLOCK / 65000, TIM_CLOCK / 100);
+
+	palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_ALTERNATE(HW_ICU_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING);
+
+	HW_ICU_TIM_CLK_EN();
+
+	HW_ICU_TIMER->CR1 = 0;
+	HW_ICU_TIMER->ARR = (uint16_t)((uint32_t)TIM_CLOCK / (uint32_t)freq_hz);
+	HW_ICU_TIMER->PSC = (uint16_t)((168000000 / 2) / TIM_CLOCK) - 1;
+	HW_ICU_TIMER->EGR = TIM_PSCReloadMode_Immediate;
+
+	if (HW_ICU_CHANNEL == ICU_CHANNEL_1) {
+		HW_ICU_TIMER->CCER = TIM_OutputState_Enable;
+		HW_ICU_TIMER->CCMR1 = TIM_OCMode_PWM1 | TIM_OCPreload_Enable;
+		HW_ICU_TIMER->CCR1 = 0;
+	} else if (HW_ICU_CHANNEL == ICU_CHANNEL_2) {
+		HW_ICU_TIMER->CCER = (TIM_OutputState_Enable << 4);
+		HW_ICU_TIMER->CCMR1 = (TIM_OCMode_PWM1 << 8) | (TIM_OCPreload_Enable << 8);
+		HW_ICU_TIMER->CCR2 = 0;
+	}
+
+	HW_ICU_TIMER->CR1 |= TIM_CR1_ARPE;
+
+	pwm_servo_set_servo_out(0.5);
+
+	HW_ICU_TIMER->CR1 |= TIM_CR1_CEN;
+
+	m_is_running = true;
+
+	return freq_hz;
+}
+
+void pwm_servo_init_servo(void) {
+	pwm_servo_init(SERVO_OUT_RATE_HZ);
+}
+
+void pwm_servo_stop(void) {
+	if (m_is_running) {
+		palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT);
+	}
+
+	TIM_DeInit(HW_ICU_TIMER);
+	m_is_running = false;
+}
+
+float pwm_servo_set_duty(float duty) {
+	if (!m_is_running) {
+		return -1.0;
+	}
+
+	utils_truncate_number(&duty, 0.0, 1.0);
+
+	uint32_t output = (uint32_t)((float)HW_ICU_TIMER->ARR * duty);
+
+	if (HW_ICU_CHANNEL == ICU_CHANNEL_1) {
+		HW_ICU_TIMER->CCR1 = output;
+	} else if (HW_ICU_CHANNEL == ICU_CHANNEL_2) {
+		HW_ICU_TIMER->CCR2 = output;
+	}
+
+	return (float)output / (float)HW_ICU_TIMER->ARR;
+
+}
+
+void pwm_servo_set_servo_out(float output) {
+	if (!m_is_running) {
+		return;
+	}
+
+	utils_truncate_number(&output, 0.0, 1.0);
+
+	float us = (float)SERVO_OUT_PULSE_MIN_US + output *
+			(float)(SERVO_OUT_PULSE_MAX_US - SERVO_OUT_PULSE_MIN_US);
+	us *= (float)TIM_CLOCK / 1000000.0;
+
+	if (HW_ICU_CHANNEL == ICU_CHANNEL_1) {
+		HW_ICU_TIMER->CCR1 = (uint32_t)us;
+	} else if (HW_ICU_CHANNEL == ICU_CHANNEL_2) {
+		HW_ICU_TIMER->CCR2 = (uint32_t)us;
+	}
+}
+
+bool pwm_servo_is_running(void) {
+	return m_is_running;
+}
+
+#pragma GCC pop_options

driver/servo_simple.h → driver/pwm_servo.h

@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 - 2021 Benjamin Vedder	[email protected]
+	Copyright 2024 Benjamin Vedder	[email protected]
 
 	This file is part of the VESC firmware.
 
@@ -17,15 +17,17 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
     */
 
-#ifndef SERVO_SIMPLE_H_
-#define SERVO_SIMPLE_H_
+#ifndef DRIVER_PWM_SERVO_H_
+#define DRIVER_PWM_SERVO_H_
 
+#include <stdint.h>
 #include <stdbool.h>
 
-// Functions
-void servo_simple_init(void);
-void servo_simple_stop(void);
-bool servo_simple_is_running(void);
-void servo_simple_set_output(float out);
+uint32_t pwm_servo_init(uint32_t freq_hz);
+void pwm_servo_init_servo(void);
+void pwm_servo_stop(void);
+float pwm_servo_set_duty(float duty);
+void pwm_servo_set_servo_out(float output);
+bool pwm_servo_is_running(void);
 
-#endif /* SERVO_SIMPLE_H_ */
+#endif /* DRIVER_PWM_SERVO_H_ */