PWM output for channels 1-4 for lisa stm32 passthrough

conf/autopilot/lisa_passthrough.makefile

@@ -56,10 +56,14 @@ pt.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200
 pt.CFLAGS += -DRADIO_CONTROL_LINK=Uart3
 
 # Actuators
-pt.srcs += $(SRC_BOOZ)/actuators/booz_supervision.c
+#pt.srcs += $(SRC_BOOZ)/actuators/booz_supervision.c
-pt.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
+#pt.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
-pt.srcs += $(SRC_BOOZ)/actuators/booz_actuators_asctec.c
+#pt.srcs += $(SRC_BOOZ)/actuators/booz_actuators_asctec.c
-pt.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+#pt.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
+#
+#pt.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
+#pt.CFLAGS += -DUSE_I2C1
 
-pt.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
+# PWM actuator
-pt.CFLAGS += -DUSE_I2C1
+pt.srcs += $(SRC_BOOZ)/actuators/booz_actuators_pwm.c
+pt.srcs += $(SRC_BOOZ_ARCH)/actuators/booz_actuators_pwm_hw.c

sw/airborne/booz/arch/stm32/actuators/booz_actuators_pwm_hw.c

@@ -29,11 +29,12 @@
 #include <stm32/misc.h>
 #include <stm32/tim.h>
 
+#define PCLK 72000000
+#define ONE_MHZ_CLK 1000000
+#define SERVO_HZ 40
+
 void booz_actuators_pwm_hw_init(void) {
 
-  /* System clock */
-  /* PCLK1 = HCLK/4 */
-  RCC_PCLK1Config(RCC_HCLK_Div4);
   /* TIM3 clock enable */
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
   /* GPIOB and GPIOC clock enable */
@@ -52,50 +53,44 @@ void booz_actuators_pwm_hw_init(void) {
   TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
   TIM_OCInitTypeDef  TIM_OCInitStructure;
   /* Time base configuration */
-  TIM_TimeBaseStructure.TIM_Period = 56249;
+  TIM_TimeBaseStructure.TIM_Prescaler = (PCLK / ONE_MHZ_CLK) - 1; // 1uS
-  TIM_TimeBaseStructure.TIM_Prescaler = 15;
+  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);
  
-  uint16_t CCR1_Val = 2250;
+  /* PWM1 Mode configuration: All Channels */
-  uint16_t CCR2_Val = 2250;
-  uint16_t CCR3_Val = 2250;
-  /* PWM1 Mode configuration: Channel1 */
   TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
   TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
-  TIM_OCInitStructure.TIM_Pulse = CCR1_Val;
+  TIM_OCInitStructure.TIM_Pulse = 0; // default low (no pulse)
   TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
 
+  /* PWM1 Mode configuration: Channel1 */
   TIM_OC1Init(TIM3, &TIM_OCInitStructure);
-
   TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
   /* PWM1 Mode configuration: Channel2 */
-  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
-  TIM_OCInitStructure.TIM_Pulse = CCR2_Val;
-
   TIM_OC2Init(TIM3, &TIM_OCInitStructure);
-
   TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
   /* PWM1 Mode configuration: Channel3 */
-  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
-  TIM_OCInitStructure.TIM_Pulse = CCR3_Val;
-
   TIM_OC3Init(TIM3, &TIM_OCInitStructure);
-
   TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
 
+  /* PWM1 Mode configuration: Channel4 */
+  TIM_OC4Init(TIM3, &TIM_OCInitStructure);
+  TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
+
   /* TIM3 enable counter */
   TIM_Cmd(TIM3, ENABLE);
 
 }
 
+/* set pulse widths from actuator values, assumed to be in us */
 void booz_actuators_pwm_commit(void) {
-  TIM_SetCompare1 (TIM3, booz_actuators_pwm_values[0]);
+  TIM_SetCompare1(TIM3, booz_actuators_pwm_values[0]);
-  TIM_SetCompare2 (TIM3, booz_actuators_pwm_values[1]);
+  TIM_SetCompare2(TIM3, booz_actuators_pwm_values[1]);
-  TIM_SetCompare3 (TIM3, booz_actuators_pwm_values[2]);
+  TIM_SetCompare3(TIM3, booz_actuators_pwm_values[2]);
-  
+  TIM_SetCompare4(TIM3, booz_actuators_pwm_values[3]);
 }

sw/airborne/fms/fms_autopilot_msg.h

@@ -5,6 +5,8 @@
 #include "math/pprz_algebra_int.h"
 #include "airframe.h"
 
+#define LISA_PWM_NB 6
+
 /*
  * Testing
  */
@@ -92,11 +94,7 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp
 
 struct __attribute__ ((packed)) AutopilotMessagePTDown
 {
-  int16_t command_pitch;
+  uint16_t pwm_outputs_usecs[LISA_PWM_NB];
-  int16_t command_roll;
-  int16_t command_yaw;
-  int16_t command_thrust;
-  int16_t actuators[SERVOS_NB];
 };
 
 union AutopilotMessagePT

sw/airborne/fms/fms_spi_autopilot_msg.c

@@ -138,8 +138,8 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
 
 static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_out)
 {
-  for (int i = 0; i < SERVOS_NB; i++) {
+  for (int i = 0; i < LISA_PWM_NB; i++) {
-    msg_out->actuators[i] = actuators[i];
+    msg_out->pwm_outputs_usecs[i] = actuators[i];
   }
 }