Converted to deck, choice from multiple pins

src/drivers/interface/servo.h → src/deck/drivers/interface/servo.h

@@ -38,18 +38,12 @@
 #include "config.h"
 
 /******** Defines ********/
-#define SERVO_PWM_BITS           (0)
 #define SERVO_PWM_PERIOD         1000  // ARR register content
 #define SERVO_PWM_FREQUENCY_HZ   50 // target servo pwm frequency
 #define SERVO_PWM_PRESCALE       (uint16_t) (1680) // 84mhz / (50hz * ARR)
-#define SERVO_BASE_FREQ          (0) // should be calculated
-#define SERVO_ANGLE_ZERO         130
-#define SERVO_ANGLE_LIMIT        30
-/*** Public interface ***/
 
 /**
- * Servo Initialization. Configures two output compare channels in PWM mode
- * with one of them inverted to increase power output.
+ * Servo Initialization
  */
 void servoInit();
 
@@ -58,7 +52,7 @@ bool servoTest(void);
 /**
  *
  * @brief Set servo angle.
- * @param: angle: desired servo angle
+ * @param: angle: desired servo angle in degrees
  */
 void servoSetAngle(uint8_t angle);
 
@@ -69,8 +63,9 @@ void servoSetAngle(uint8_t angle);
 void servoAngleCallBack(void);
 
 /**
- * Saturate servo angle. Limit is defined by macro: SERVO_ANGLE_LIMIT
+ * Saturate servo angle. Min is 0, max is defined by parameter 'servoRange'
  * @param angle: pointer to desired angle
  * */
-uint8_t saturateAngle(int8_t angle);
-#endif /* __MOTORS_H__ */
+uint8_t saturateAngle(uint8_t angle);
+
+#endif /* __SERVO_H__ */

src/deck/drivers/src/Kbuild

@@ -15,6 +15,7 @@ obj-$(CONFIG_DECK_LOCO)                 += lpsTdoa3Tag.o
 obj-$(CONFIG_DECK_LOCO)                 += lpsTwrTag.o
 obj-$(CONFIG_DECK_MULTIRANGER)          += multiranger.o
 obj-$(CONFIG_DECK_OA)                   += oa.o
+obj-$(CONFIG_DECK_SERVO)                += servo.o
 obj-$(CONFIG_DECK_USD)                  += usddeck.o
 obj-$(CONFIG_DECK_ZRANGER)              += zranger.o
 obj-$(CONFIG_DECK_ZRANGER2)             += zranger2.o

src/deck/drivers/src/Kconfig

@@ -390,6 +390,37 @@ config DECK_OA
     help
         Codename: Obstacle Avoidance driver, obsolete deck.
 
+config DECK_SERVO
+    bool "Support the servo deck (prototype/via BigQuad deck)"
+    default n
+    help
+      Servo PWM driver.
+
+choice
+    prompt "Servo pin"
+    depends on DECK_SERVO
+    default DECK_SERVO_USE_TX2
+
+    config DECK_SERVO_USE_TX2
+        bool "PA2/TX2"
+
+    config DECK_SERVO_USE_RX2
+        bool "PA3/RX2"
+
+    config DECK_SERVO_USE_IO1
+        bool "PB8/IO_1"
+
+    config DECK_SERVO_USE_IO2
+        bool "PB5/IO_2"
+
+    config DECK_SERVO_USE_IO3
+        bool "PB4/IO_3"
+
+    config DECK_SERVO_USE_MOSI
+        bool "PA7/MOSI"
+
+endchoice
+
 config DECK_USD
     bool "Support the Micro SD card deck"
     default y

src/deck/drivers/src/servo.c

@@ -0,0 +1,233 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2024 Bitcraze AB
+ *
+ * This program 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, in version 3.
+ *
+ * This program 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/>.
+ *
+ * servo.c - Servo driver
+ *
+ * Using contributions from: Eric Ewing, Will Wu
+ */
+#define DEBUG_MODULE "SERVO"
+
+#include <stdbool.h>
+
+/* ST includes */
+#include "stm32fxxx.h"
+
+//FreeRTOS includes
+#include "FreeRTOS.h"
+#include "task.h"
+#include "debug.h"
+#include <string.h>
+#include <inttypes.h>
+#include "motors.h"
+#include "param.h"
+#include "deck.h"
+
+static uint16_t servo_MIN_us = 1000;
+static uint16_t servo_MAX_us = 2000;
+
+#include "servo.h"
+
+static bool isInit = false;
+
+const MotorPerifDef* servoMap;
+extern const MotorPerifDef* servoMapIO1;
+extern const MotorPerifDef* servoMapIO2;
+extern const MotorPerifDef* servoMapIO3;
+extern const MotorPerifDef* servoMapRX2;
+extern const MotorPerifDef* servoMapTX2;
+extern const MotorPerifDef* servoMapMOSI;
+
+/* Public functions */
+static uint8_t servo_idle = 90;
+static uint8_t s_servo_angle;
+static uint8_t servo_range = 180; // in degrees
+
+void servoMapInit(const MotorPerifDef* servoMapSelect)
+{
+  servoMap = servoMapSelect;
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+  //clock the servo pin and the timers
+  RCC_AHB1PeriphClockCmd(servoMap->gpioPerif, ENABLE);
+  RCC_APB1PeriphClockCmd(servoMap->timPerif, ENABLE);
+
+  //configure gpio for timer out
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Pin = servoMap->gpioPin;
+  GPIO_Init(servoMap->gpioPort, &GPIO_InitStructure);
+
+  //map timer to alternate function
+  GPIO_PinAFConfig(servoMap->gpioPort, servoMap->gpioPinSource, servoMap->gpioAF);
+
+  //Timer configuration
+  TIM_TimeBaseStructure.TIM_Period = SERVO_PWM_PERIOD;
+  TIM_TimeBaseStructure.TIM_Prescaler = SERVO_PWM_PRESCALE;
+  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+  TIM_TimeBaseInit(servoMap->tim, &TIM_TimeBaseStructure);
+
+  // PWM channels configuration
+  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+  TIM_OCInitStructure.TIM_Pulse = 0;
+  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+  // Configure OC1
+  servoMap->ocInit(servoMap->tim, &TIM_OCInitStructure);
+  servoMap->preloadConfig(servoMap->tim, TIM_OCPreload_Enable);
+
+
+  //Enable the timer PWM outputs
+  TIM_CtrlPWMOutputs(servoMap->tim, ENABLE);
+  servoMap->setCompare(servoMap->tim, 0x00);
+
+  //Enable the timer
+  TIM_Cmd(servoMap->tim, ENABLE);
+}
+
+void servoInit()
+{
+  if (isInit){
+    return;
+  }
+
+  #ifdef CONFIG_DECK_SERVO_USE_IO1
+    servoMapInit(servoMapIO1);
+    DEBUG_PRINT("Init on IO1 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_IO2
+    servoMapInit(servoMapIO2);
+    DEBUG_PRINT("Init on IO2 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_IO3
+    servoMapInit(servoMapIO3);
+    DEBUG_PRINT("Init on IO3 [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_RX2
+    servoMapInit(servoMapRX2);
+    DEBUG_PRINT("Init on RX2 [OK]\n"); // not working on Bolt 1.1...
+  #elif CONFIG_DECK_SERVO_USE_MOSI
+    servoMapInit(servoMapMOSI);
+    DEBUG_PRINT("Init on MOSI [OK]\n");
+  #elif CONFIG_DECK_SERVO_USE_TX2
+    servoMapInit(servoMapTX2);
+    DEBUG_PRINT("Init on TX2 [OK]\n"); // not working on Bolt 1.1...
+  #else
+    isInit = false
+    DEBUG_PRINT("Failed to configure servo pin!\n");
+    return;
+  #endif
+
+  servoSetAngle(saturateAngle(servo_idle));
+
+  s_servo_angle = servo_idle;
+
+  isInit = true;
+}
+
+bool servoTest(void)
+{
+  return isInit;
+}
+
+void servoSetAngle(uint8_t angle)
+{
+  // set CCR register
+  // Duty% = CCR/ARR*100, so CCR = Duty%/100 * ARR
+
+  double pulse_length_us = (double)(angle) / servo_range * (servo_MAX_us - servo_MIN_us) + servo_MIN_us;
+  double pulse_length_s = pulse_length_us / 1000000;
+  const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ);
+  servoMap->setCompare(servoMap->tim, ccr_val);
+  DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us);
+
+}
+
+uint8_t saturateAngle(uint8_t angle)
+{
+  if (angle > servo_range) {
+    return servo_range;
+  }
+  else if (angle < 0) {
+    return 0;
+  }
+  else {
+    return angle;
+  }
+
+}
+
+void servoAngleCallBack(void)
+{
+  servoSetAngle(saturateAngle(s_servo_angle));
+}
+
+static const DeckDriver servo_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcServo",
+
+  #ifdef CONFIG_DECK_SERVO_USE_IO1
+    .usedPeriph = DECK_USING_TIMER4,
+    .usedGpio = DECK_USING_IO_1,
+  #elif CONFIG_DECK_SERVO_USE_IO2
+    .usedPeriph = DECK_USING_TIMER3,
+    .usedGpio = DECK_USING_IO_2,
+  #elif CONFIG_DECK_SERVO_USE_IO3
+    .usedPeriph = DECK_USING_TIMER3,
+    .usedGpio = DECK_USING_IO_3,
+  #elif CONFIG_DECK_SERVO_USE_RX2
+    .usedPeriph = DECK_USING_TIMER5,
+    .usedGpio = DECK_USING_PA3,
+  #elif CONFIG_DECK_SERVO_USE_MOSI
+    .usedPeriph = DECK_USING_TIMER14,
+    .usedGpio = DECK_USING_PA7,
+  #elif CONFIG_DECK_SERVO_USE_TX2
+    .usedPeriph = DECK_USING_TIMER5,
+    .usedGpio = DECK_USING_PA2,
+  #else
+    .usedPeriph = 0,
+    .usedGpio = 0,
+  #endif
+
+  .init = servoInit,
+  .test = servoTest,
+};
+
+DECK_DRIVER(servo_deck);
+
+PARAM_GROUP_START(servo)
+
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit)
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMINus, &servo_MIN_us)
+PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMAXus, &servo_MAX_us)
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoRange, &servo_range)
+PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoIdle, &servo_idle)
+PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack)
+
+PARAM_GROUP_STOP(servo)

src/deck/interface/deck_core.h

@@ -69,6 +69,7 @@ bool deckTest(void);
 #define DECK_USING_TIMER10 (1 << 16)
 #define DECK_USING_TIMER14 (1 << 15)
 #define DECK_USING_TIMER9  (1 << 17)
+#define DECK_USING_TIMER4  (1 << 18)
 
 struct deckInfo_s;
 struct deckFwUpdate_s;

src/drivers/src/Kbuild

@@ -17,7 +17,6 @@ obj-y += nvic.o
 obj-y += pca9685.o
 obj-y += piezo.o
 obj-y += pmw3901.o
-obj-y += servo.o
 obj-y += swd.o
 obj-y += uart1.o
 obj-y += uart2.o

src/drivers/src/motors_def.c

@@ -710,28 +710,6 @@ static const MotorPerifDef MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD =
     .preloadConfig = TIM_OC1PreloadConfig,
 };
 
-// Deck MOSI, PA7, TIM14_CH1, Servo
-static const MotorPerifDef MOTORS_PA7_TIM14_CH1_SERVO =
-{
-    .drvType       = BRUSHLESS,
-    .gpioPerif     = RCC_AHB1Periph_GPIOA,
-    .gpioPort      = GPIOA,
-    .gpioPin       = GPIO_Pin_7,
-    .gpioPinSource = GPIO_PinSource7,
-    .gpioOType     = GPIO_OType_PP,
-    .gpioAF        = GPIO_AF_TIM14,
-    .timPerif      = RCC_APB1Periph_TIM14,
-    .tim           = TIM14,
-    .timPolarity   = TIM_OCPolarity_High,
-    .timDbgStop    = DBGMCU_TIM14_STOP,
-    .timPeriod     = MOTORS_BL_PWM_PERIOD,
-    .timPrescaler  = MOTORS_BL_PWM_PRESCALE,
-    .setCompare    = TIM_SetCompare1,
-    .getCompare    = TIM_GetCapture1,
-    .ocInit        = TIM_OC1Init,
-    .preloadConfig = TIM_OC1PreloadConfig,
-};
-
 /**
  * Mapping for Tags that don't have motors.
  * Actually same mapping as for CF2 but the pins are not physically connected.
@@ -830,4 +808,29 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] =
 /**
  * Servo mapped to the Bigquad CPPM (MOSI) port
  */
-const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_SERVO;
+const MotorPerifDef* servoMapMOSI = &MOTORS_PA7_TIM14_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M1 / TX2 port
+ */
+const MotorPerifDef* servoMapTX2 = &MOTORS_PA2_TIM5_CH3_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M3 / IO2 port
+ */
+const MotorPerifDef* servoMapIO2 = &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M2 / IO3 port
+ */
+const MotorPerifDef* servoMapIO3 = &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to the Bigquad M4 / RX2 port
+ */
+const MotorPerifDef* servoMapRX2 = &MOTORS_PA3_TIM5_CH4_BRUSHLESS_OD;
+
+/**
+ * Servo mapped to IO1 port
+ */
+const MotorPerifDef* servoMapIO1 = &MOTORS_PB8_TIM4_CH3_BRUSHLESS_OD;