From 276a4d0778c48a505dfdd91c9842b7388c2fed6a Mon Sep 17 00:00:00 2001
From: Allen Ibara <uce.savannah@ibara.net>
Date: Thu, 15 Jul 2010 18:04:37 +0000
Subject: [PATCH] Enable SERVO5/SERVO6 on TIM4 for lisa passthrough

---
 .../booz/actuators/booz_actuators_pwm.h       |  2 +-
 .../stm32/actuators/booz_actuators_pwm_hw.c   | 45 ++++++++++++++-----
 2 files changed, 36 insertions(+), 11 deletions(-)

diff --git a/sw/airborne/booz/actuators/booz_actuators_pwm.h b/sw/airborne/booz/actuators/booz_actuators_pwm.h
index 47845b26ab..ed8b868c72 100644
--- a/sw/airborne/booz/actuators/booz_actuators_pwm.h
+++ b/sw/airborne/booz/actuators/booz_actuators_pwm.h
@@ -26,7 +26,7 @@
 
 #include "std.h"
 
-#define BOOZ_ACTUATORS_PWM_NB 4
+#define BOOZ_ACTUATORS_PWM_NB 6
 extern int32_t booz_actuators_pwm_values[BOOZ_ACTUATORS_PWM_NB];
 
 #include "actuators/booz_actuators_pwm_hw.h"
diff --git a/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_hw.c b/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_hw.c
index 18ec47efad..9a8e81cf5b 100644
--- a/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_hw.c
+++ b/sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_hw.c
@@ -35,56 +35,79 @@
 
 void booz_actuators_pwm_hw_init(void) {
 
-  /* TIM3 clock enable */
+  /* TIM3 and TIM4 clock enable */
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
+
   /* GPIOB and GPIOC clock enable */
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | 
 			 RCC_APB2Periph_AFIO, ENABLE);
-  /* GPIO */
+  /* GPIO C */
   /* PC6=servo1 PC7=servo2 PC8=servo3 PC9=servo4 */
   GPIO_InitTypeDef GPIO_InitStructure;
   GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
   GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF_PP;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_Init(GPIOC, &GPIO_InitStructure);
+  /* need to remate alternate function, pins 37, 38, 39, 40 on LQFP64 */
   GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE);	
 
-  /* TIM */
-  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-  TIM_OCInitTypeDef  TIM_OCInitStructure;
+  /* GPIO B */
+  /* PB8=servo5 PB9=servo6 */
+  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_8 | GPIO_Pin_9;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
   /* Time base configuration */
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   TIM_TimeBaseStructure.TIM_Prescaler = (PCLK / ONE_MHZ_CLK) - 1; // 1uS
   TIM_TimeBaseStructure.TIM_Period = (ONE_MHZ_CLK / SERVO_HZ) - 1;
   TIM_TimeBaseStructure.TIM_ClockDivision = 0;
   TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 
   TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
  
   /* PWM1 Mode configuration: All Channels */
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
   TIM_OCInitStructure.TIM_Pulse = 0; // default low (no pulse)
   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
 
-  /* PWM1 Mode configuration: Channel1 */
+  /* PWM1 Mode configuration: TIM3 Channel1 */
   TIM_OC1Init(TIM3, &TIM_OCInitStructure);
   TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
-  /* PWM1 Mode configuration: Channel2 */
+  /* PWM1 Mode configuration: TIM3 Channel2 */
   TIM_OC2Init(TIM3, &TIM_OCInitStructure);
   TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
-  /* PWM1 Mode configuration: Channel3 */
+  /* PWM1 Mode configuration: TIM3 Channel3 */
   TIM_OC3Init(TIM3, &TIM_OCInitStructure);
   TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
-  /* PWM1 Mode configuration: Channel4 */
+  /* PWM1 Mode configuration: TIM3 Channel4 */
   TIM_OC4Init(TIM3, &TIM_OCInitStructure);
   TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
-  /* TIM3 enable counter */
+  /* PWM1 Mode configuration: TIM4 Channel3 */
+  TIM_OC3Init(TIM4, &TIM_OCInitStructure);
+  TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
+
+  /* PWM1 Mode configuration: TIM4 Channel4 */
+  TIM_OC4Init(TIM4, &TIM_OCInitStructure);
+  TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
+
+  /* TIM3 enable */
+  TIM_ARRPreloadConfig(TIM3, ENABLE);
+  TIM_CtrlPWMOutputs(TIM3, ENABLE);
   TIM_Cmd(TIM3, ENABLE);
 
+  /* TIM4 enable */
+  TIM_ARRPreloadConfig(TIM4, ENABLE);
+  TIM_CtrlPWMOutputs(TIM4, ENABLE);
+  TIM_Cmd(TIM4, ENABLE);
+
 }
 
 /* set pulse widths from actuator values, assumed to be in us */
@@ -93,4 +116,6 @@ void booz_actuators_pwm_commit(void) {
   TIM_SetCompare2(TIM3, booz_actuators_pwm_values[1]);
   TIM_SetCompare3(TIM3, booz_actuators_pwm_values[2]);
   TIM_SetCompare4(TIM3, booz_actuators_pwm_values[3]);
+  TIM_SetCompare3(TIM4, booz_actuators_pwm_values[4]);
+  TIM_SetCompare4(TIM4, booz_actuators_pwm_values[5]);
 }