diff --git a/src/drivers/interface/servo.h b/src/deck/drivers/interface/servo.h similarity index 79% rename from src/drivers/interface/servo.h rename to src/deck/drivers/interface/servo.h index e17636a9e6..bc58675d09 100644 --- a/src/drivers/interface/servo.h +++ b/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__ */ diff --git a/src/deck/drivers/src/Kbuild b/src/deck/drivers/src/Kbuild index 79ff5d316b..52765dfab6 100644 --- a/src/deck/drivers/src/Kbuild +++ b/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 diff --git a/src/deck/drivers/src/Kconfig b/src/deck/drivers/src/Kconfig index 1db2aa01b4..1c4ee4ff58 100644 --- a/src/deck/drivers/src/Kconfig +++ b/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 diff --git a/src/deck/drivers/src/servo.c b/src/deck/drivers/src/servo.c new file mode 100644 index 0000000000..8f04e2aaa1 --- /dev/null +++ b/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 . + * + * servo.c - Servo driver + * + * Using contributions from: Eric Ewing, Will Wu + */ +#define DEBUG_MODULE "SERVO" + +#include + +/* ST includes */ +#include "stm32fxxx.h" + +//FreeRTOS includes +#include "FreeRTOS.h" +#include "task.h" +#include "debug.h" +#include +#include +#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) diff --git a/src/deck/interface/deck_core.h b/src/deck/interface/deck_core.h index c13deaccd5..47dc23d867 100644 --- a/src/deck/interface/deck_core.h +++ b/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; diff --git a/src/drivers/src/Kbuild b/src/drivers/src/Kbuild index b496872bfb..18bc093636 100644 --- a/src/drivers/src/Kbuild +++ b/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 diff --git a/src/drivers/src/motors_def.c b/src/drivers/src/motors_def.c index 4f84e7aa86..93abbb1a99 100644 --- a/src/drivers/src/motors_def.c +++ b/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; diff --git a/src/drivers/src/servo.c b/src/drivers/src/servo.c deleted file mode 100644 index c7fd1d38f8..0000000000 --- a/src/drivers/src/servo.c +++ /dev/null @@ -1,171 +0,0 @@ -/** - * || ____ _ __ - * +------+ / __ )(_) /_______________ _____ ___ - * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ - * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ - * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ - * - * Crazyflie control firmware - * - * Copyright (C) 2011-2012 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 . - * - * servo.c - Servo driver - * - * This code mainly interfacing the PWM peripheral lib of ST. - * Author: Eric Ewing, Will Wu - */ -#define DEBUG_MODULE "SERVO" -# -#include - -/* ST includes */ -#include "stm32fxxx.h" - -//FreeRTOS includes -#include "FreeRTOS.h" -#include "task.h" -#include "debug.h" -#include -#include -#include "motors.h" -#include "param.h" -#include "deck.h" - -// TODO Verify PWM settings -static const double SERVO_ZERO_PULSE_ms = 0.5; -static const double SERVO_180_PULSE_ms = 2.5; - -#include "servo.h" - -static bool isInit = false; -// we use the "servoMapMOSI" struct to initialize PWM -extern const MotorPerifDef* servoMapMOSI; - -/* Public functions */ -static int8_t s_servo_angle = 0; - -void servoInit() -{ - if (isInit){ - return; - } - - GPIO_InitTypeDef GPIO_InitStructure; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_OCInitTypeDef TIM_OCInitStructure; - - //clock the servo pin and the timers - RCC_AHB1PeriphClockCmd(servoMapMOSI->gpioPerif, ENABLE); - RCC_APB1PeriphClockCmd(servoMapMOSI->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; // TODO: verify this - GPIO_InitStructure.GPIO_Pin = servoMapMOSI->gpioPin; - GPIO_Init(servoMapMOSI->gpioPort, &GPIO_InitStructure); - - //map timer to alternate function - GPIO_PinAFConfig(servoMapMOSI->gpioPort, servoMapMOSI->gpioPinSource, servoMapMOSI->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(servoMapMOSI->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 - servoMapMOSI->ocInit(servoMapMOSI->tim, &TIM_OCInitStructure); - servoMapMOSI->preloadConfig(servoMapMOSI->tim, TIM_OCPreload_Enable); - - - //Enable the timer PWM outputs - TIM_CtrlPWMOutputs(servoMapMOSI->tim, ENABLE); - servoMapMOSI->setCompare(servoMapMOSI->tim, 0x00); - - //Enable the timer - TIM_Cmd(servoMapMOSI->tim, ENABLE); - - DEBUG_PRINT("Init [OK]\n"); - servoSetAngle(SERVO_ANGLE_ZERO); - - 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_ms = (double)(angle) / 180. * (SERVO_180_PULSE_ms - SERVO_ZERO_PULSE_ms) + SERVO_ZERO_PULSE_ms; - double pulse_length_s = pulse_length_ms / 1000; - const uint32_t ccr_val = (uint32_t)(pulse_length_s * SERVO_PWM_PERIOD * SERVO_PWM_FREQUENCY_HZ); - servoMapMOSI->setCompare(servoMapMOSI->tim, ccr_val); - DEBUG_PRINT("[SERVO]: Set Angle: %u, set ratio: %" PRId32 "\n", angle, ccr_val); - -} - -uint8_t saturateAngle(int8_t angle) -{ - if (angle > SERVO_ANGLE_LIMIT || angle < -SERVO_ANGLE_LIMIT) { -// DEBUG_PRINT("[SERVO]: Servo angle out of range! Capping...\n"); - bool sign = angle > 0; - return sign ? SERVO_ANGLE_LIMIT + SERVO_ANGLE_ZERO : -SERVO_ANGLE_LIMIT+SERVO_ANGLE_ZERO; - } - else { - return angle + SERVO_ANGLE_ZERO; - } - -} - -void servoAngleCallBack(void) -{ - servoSetAngle(saturateAngle(s_servo_angle)); -} - -static const DeckDriver servo_deck = { - .vid = 0x00, - .pid = 0x00, - .name = "bcServo", - - .usedPeriph = 0, - .usedGpio = 0, - .init = servoInit, - .test = 0, -}; - -DECK_DRIVER(servo_deck); - -PARAM_GROUP_START(servo) - -PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, servoInitialized, &isInit) -PARAM_ADD_WITH_CALLBACK(PARAM_UINT8 , servoAngle, &s_servo_angle, &servoAngleCallBack) - -PARAM_GROUP_STOP(servo)