add firmware

drv/pwm.cpp

@@ -0,0 +1,85 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+#include "pwm.h"
+
+bool PWM_Enable=false;
+short PWM_work_min = 1050;//???
+
+inline static unsigned short Mid(unsigned short Val, unsigned short Min, unsigned short Max)
+{
+  if(Val<Min) return Min;
+  if(Val>Max) return Max;
+  return Val;
+}
+
+
+void PWM_Init(unsigned long Freq)
+{
+  GPIO_InitPin(GPIO_PIN_0 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_1 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_2 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  GPIO_InitPin(GPIO_PIN_3 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF1);
+  
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
+  TIM2->CR2 = 0;
+  
+
+  
+  TIM2->ARR = 1000 - 1; //The comparison range is 100     
+  TIM2->PSC = (16'000'000/1000/16000)-1; // The divider is set to 16000Hz
+  
+  TIM2->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1PE; //PWM mode 1 for CH1
+  TIM2->CCMR1|= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2PE; // PWM mode 1 for CH2
+  TIM2->CCMR2 = TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3PE; // PWM mode 1 for  CH3
+  TIM2->CCMR2|= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4PE; // PWM mode 1 for CH4
+  
+  TIM2->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E |TIM_CCER_CC4E; // CH1 and CH2 outputs are active
+  TIM2->BDTR = TIM_BDTR_MOE; // Allow active outputs to work
+  TIM2->CR1 = TIM_CR1_CEN; // Timer is running
+  for(int a=0; a<100000; a++) { asm volatile("NOP");}
+  
+}
+//------------------------------------------------------------------------------
+
+static void Stop()
+{
+  PWM_Enable=false;
+
+  TIM2->CCER=0;
+
+}
+//------------------------------------------------------------------------------
+
+void PWM_SetQuad(short Pow[4], unsigned short Min, unsigned short Max)
+{
+  if(!Pow) { Stop(); return; }
+  TIM2->CCR1 = Mid(Pow[0], Min, Max); //M1
+  TIM2->CCR2 = Mid(Pow[1], Min, Max); //M2
+  TIM2->CCR3 = Mid(Pow[2], Min, Max); //M3
+  TIM2->CCR4 = Mid(Pow[3], Min, Max); //M4
+  
+  
+
+  bool en=false;
+  for(int a=0; a<4; a++) if(Pow[a]>PWM_work_min) en=true;
+  PWM_Enable=en;
+}
+//------------------------------------------------------------------------------
+
+
+
+void PWM_SetAll(unsigned short Pow)
+{
+
+  TIM2->CCR1 = Pow; 
+  TIM2->CCR2 = Pow; 
+  TIM2->CCR3 = Pow; 
+  TIM2->CCR4 = Pow; 
+  
+  bool en=false;
+  if(Pow>PWM_work_min) en=true;
+  PWM_Enable=en;
+}
+//------------------------------------------------------------------------------