diff --git a/src/deck/drivers/interface/servo.h b/src/deck/drivers/interface/servo.h new file mode 100644 index 0000000000..bc58675d09 --- /dev/null +++ b/src/deck/drivers/interface/servo.h @@ -0,0 +1,71 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 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.h - servo header file + * + * The motors PWM ratio is a value on 16 bits (from 0 to 0xFFFF) + * the functions of the driver will make the conversions to the actual PWM + * precision (ie. if the precision is 8bits 0xFFFF and 0xFF00 are equivalents). + * + * The precision is set in number of bits by the define MOTORS_PWM_BITS + * The timer prescaler is set by MOTORS_PWM_PRESCALE + */ +#ifndef __SERVO_H__ +#define __SERVO_H__ + +#include +#include +#include "config.h" + +/******** Defines ********/ +#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) + +/** + * Servo Initialization + */ +void servoInit(); + +bool servoTest(void); + +/** + * + * @brief Set servo angle. + * @param: angle: desired servo angle in degrees + */ +void servoSetAngle(uint8_t angle); + +/** + * Servo angle parameter callback. When servo angle is changed, call + * function to change servo angle automatically + * */ +void servoAngleCallBack(void); + +/** + * Saturate servo angle. Min is 0, max is defined by parameter 'servoRange' + * @param angle: pointer to desired angle + * */ +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..158557c9eb --- /dev/null +++ b/src/deck/drivers/src/servo.c @@ -0,0 +1,259 @@ +/** + * || ____ _ __ + * +------+ / __ )(_) /_______________ _____ ___ + * | 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" + +// #define DEBUG_SERVO + +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); + + #ifdef DEBUG_SERVO + DEBUG_PRINT("Set Angle: %u deg, pulse width: %f us \n", angle, pulse_length_us); + #endif +} + +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(deck) + +PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcServo, &isInit) +PARAM_GROUP_STOP(deck) + +/** + * "Servo" deck parameters + */ +PARAM_GROUP_START(servo) + +/** + * @brief PWM pulse width for minimal servo position (in microseconds) + */ +PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMINus, &servo_MIN_us) +/** + * @brief PWM pulse width for maximal servo position (in microseconds) + */ +PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, servoMAXus, &servo_MAX_us) +/** + * @brief Servo range, i.e. angle between the min and max positions (in degrees) + */ +PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoRange, &servo_range) +/** + * @brief Servo idle (startup) angular position (in degrees, min = 0, max = servoRange) + */ +PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servoIdle, &servo_idle) +/** + * @brief Servo angular position (in degrees, min = 0, max = servoRange) + */ +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/motors_def.c b/src/drivers/src/motors_def.c index 0e334efdcd..d9ef9658c1 100644 --- a/src/drivers/src/motors_def.c +++ b/src/drivers/src/motors_def.c @@ -623,7 +623,7 @@ static const MotorPerifDef MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD = }; // Deck IO3, PB4, TIM3_CH1 -static const MotorPerifDef MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD = +static const MotorPerifDef MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD = { .drvType = BRUSHLESS, .gpioPerif = RCC_AHB1Periph_GPIOB, @@ -743,7 +743,7 @@ const MotorPerifDef* motorMapDefaultBrushed[NBR_OF_MOTORS] = const MotorPerifDef* motorMapBigQuadDeck[NBR_OF_MOTORS] = { &MOTORS_PA2_TIM2_CH3_BRUSHLESS_OD, - &MOTORS_PB4_TIM2_CH1_BRUSHLESS_OD, + &MOTORS_PB4_TIM3_CH1_BRUSHLESS_OD, &MOTORS_PB5_TIM3_CH2_BRUSHLESS_OD, &MOTORS_PA3_TIM2_CH4_BRUSHLESS_OD }; @@ -805,3 +805,32 @@ const MotorPerifDef* motorMapCF21Brushless[NBR_OF_MOTORS] = &MOTORS_PB10_TIM2_CH3_BRUSHLESS_OD }; +/** + * Servo mapped to the Bigquad CPPM (MOSI) port + */ +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_TIM3_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;