diff --git a/flight/Makefile b/flight/Makefile index 5581fdf1c..e4a15ac08 100644 --- a/flight/Makefile +++ b/flight/Makefile @@ -93,6 +93,7 @@ SRC += $(PIOSSTM32F10X)/pios_led.c SRC += $(PIOSSTM32F10X)/pios_usart.c SRC += $(PIOSSTM32F10X)/pios_irq.c SRC += $(PIOSSTM32F10X)/pios_adc.c +SRC += $(PIOSSTM32F10X)/pios_servo.c ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_settings.c diff --git a/flight/PiOS/Common/pios_settings.c b/flight/PiOS/Common/pios_settings.c index 107b41156..0dc5d4853 100644 --- a/flight/PiOS/Common/pios_settings.c +++ b/flight/PiOS/Common/pios_settings.c @@ -44,8 +44,10 @@ SettingsTypeDef Settings; /* String Reading: ini_gets("Section", "Key", "DefaultValue", StrBuffer, sizearray(StrBuffer), IniFile); */ void LoadSettings(void) { + /* Unused yet, until we load strings char StrBuffer[100]; long Result; + */ /* Section: GPS */ Settings.GPS.Baudrate = (uint32_t) ini_getl("GPS", "Baudrate", GPS_BAUDRATE, SETTINGS_FILE); @@ -56,6 +58,11 @@ void LoadSettings(void) /* Section: Auxillary_USART */ Settings.AuxUSART.Enabled = (bool) ini_getl("Auxillary_USART", "Enabled", AUXUART_ENABLED, SETTINGS_FILE); Settings.AuxUSART.Baudrate = (uint32_t) ini_getl("Auxillary_USART", "Baudrate", AUXUART_BAUDRATE, SETTINGS_FILE); + + /* Section: Servos */ + Settings.Servos.PositionMin = (uint16_t) ini_getl("Servos", "PositionMin", SERVOS_POSITION_MIN, SETTINGS_FILE); + Settings.Servos.PositionMin = (uint16_t) ini_getl("Servos", "PositionMin", SERVOS_POSITION_MIN, SETTINGS_FILE); + Settings.Servos.PositionMax = (uint16_t) ini_getl("Servos", "PositionMax", SERVOS_POSITION_MAX, SETTINGS_FILE); } /** diff --git a/flight/PiOS/Common/printf-stdarg.c b/flight/PiOS/Common/printf-stdarg.c index ff4e14983..4b890ae37 100644 --- a/flight/PiOS/Common/printf-stdarg.c +++ b/flight/PiOS/Common/printf-stdarg.c @@ -45,7 +45,7 @@ static void printchar(char **str, int c) ++(*str); } - else COMSendChar(1, c); // (void)putchar(c); + else COMSendChar(COM_DEBUG_UART, c); // (void)putchar(c); } diff --git a/flight/PiOS/STM32F10x/pios_servo.c b/flight/PiOS/STM32F10x/pios_servo.c new file mode 100644 index 000000000..abfca5b8c --- /dev/null +++ b/flight/PiOS/STM32F10x/pios_servo.c @@ -0,0 +1,157 @@ +/** + ****************************************************************************** + * + * @file pios_servo.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2009. + * @brief RC Servo routines + * @see The GNU Public License (GPL) Version 3 + * @defgroup PIOS_SERVO RC Servo Functions + * @{ + * + *****************************************************************************/ +/* + * 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; either version 3 of the License, or + * (at your option) any later version. + * + * 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, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +/* Project Includes */ +#include "pios.h" + + +/* Private Function Prototypes */ + + +/* Local Variables */ +static volatile uint16_t ServoPosition[SERVO_NUM_TIMER_SLOTS]; + + +/** +* Initialise Servos +*/ +void PIOS_Servo_Init(void) +{ + /* Initialise GPIOs as alternate function push/pull */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = SERVO1_PIN | SERVO2_PIN | SERVO3_PIN | SERVO4_PIN; + GPIO_Init(SERVO1TO4_PORT, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = SERVO5_PIN | SERVO6_PIN | SERVO7_PIN | SERVO8_PIN; + GPIO_Init(SERVO5TO8_PORT, &GPIO_InitStructure); + + /* Initialise RCC Clocks (TIM4 and TIM8) */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + + /* Initialise Timers TIM4 and TIM8 */ + /* With a resolution of 1uS, period of 20mS (50Hz) */ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Prescaler = (PERIPHERAL_CLOCK / 1000000) - 1; + TIM_TimeBaseStructure.TIM_Period = (20000 - 1); + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + + /* Setup each timer seperatly */ + TIM_OCInitTypeDef TIM_OCInitStructure; + + /* TIM4 */ + TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = SERVOS_POSITION_INITIAL; + TIM_OC1Init(TIM4, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); + TIM_OC2Init(TIM4, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); + TIM_OC3Init(TIM4, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); + TIM_OC4Init(TIM4, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); + TIM_ARRPreloadConfig(TIM4, ENABLE); + TIM_Cmd(TIM4, ENABLE); + + /* TIM8 */ + TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; + TIM_OCInitStructure.TIM_Pulse = SERVOS_POSITION_INITIAL; + TIM_OC1Init(TIM8, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable); + TIM_OC2Init(TIM8, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable); + TIM_OC3Init(TIM8, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable); + TIM_OC4Init(TIM8, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable); + TIM_ARRPreloadConfig(TIM8, ENABLE); + TIM_Cmd(TIM8, ENABLE); +} + +/** +* Set servo position +* \param[in] Servo Servo number (0-7) +* \param[in] Position Servo position in milliseconds +*/ +void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) +{ + /* Make sure servo exists */ + if (Servo < NUM_SERVO_OUTPUTS && Servo >= 0) + { + /* Clip servo position */ + if(Position < Settings.Servos.PositionMin) { + Position = Settings.Servos.PositionMin; + } + if(Position > Settings.Servos.PositionMax) { + Position = Settings.Servos.PositionMax; + } + + /* Update the position */ + ServoPosition[Servo] = Position; + + switch(Servo) + { + case 0: + TIM_SetCompare1(TIM4, Position); + break; + case 1: + TIM_SetCompare2(TIM4, Position); + break; + case 2: + TIM_SetCompare3(TIM4, Position); + break; + case 3: + TIM_SetCompare4(TIM4, Position); + break; + case 4: + TIM_SetCompare1(TIM8, Position); + break; + case 5: + TIM_SetCompare2(TIM8, Position); + break; + case 6: + TIM_SetCompare3(TIM8, Position); + break; + case 7: + TIM_SetCompare4(TIM8, Position); + break; + } + } +} diff --git a/flight/PiOS/inc/pios_board.h b/flight/PiOS/inc/pios_board.h index 06f44e993..c5ea942c8 100644 --- a/flight/PiOS/inc/pios_board.h +++ b/flight/PiOS/inc/pios_board.h @@ -169,17 +169,16 @@ //------------------------- // Servo outputs //------------------------- -#define SERVO1_4_PORT GPIOB +#define SERVO1TO4_PORT GPIOB #define SERVO1_PIN GPIO_Pin_6 #define SERVO2_PIN GPIO_Pin_7 #define SERVO3_PIN GPIO_Pin_8 #define SERVO4_PIN GPIO_Pin_9 -#define SERVO5_8_PORT GPIOC +#define SERVO5TO8_PORT GPIOC #define SERVO5_PIN GPIO_Pin_6 #define SERVO6_PIN GPIO_Pin_7 #define SERVO7_PIN GPIO_Pin_8 #define SERVO8_PIN GPIO_Pin_9 - #define NUM_SERVO_OUTPUTS 8 //------------------------- diff --git a/flight/PiOS/inc/pios_servo.h b/flight/PiOS/inc/pios_servo.h new file mode 100644 index 000000000..f4c3e9720 --- /dev/null +++ b/flight/PiOS/inc/pios_servo.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * + * @file pios_servo.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2009. + * @brief RC Servo functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * 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; either version 3 of the License, or + * (at your option) any later version. + * + * 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, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_SERVO_H +#define PIOS_SERVO_H + +/* Local Defines */ +#define SERVO_NUM_TIMER_SLOTS 8 +#define SERVOS_POSITION_INITIAL 1500 + +/* Public Functions */ +extern void PIOS_Servo_Init(void); +extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); + +#endif /* PIOS_SERVO_H */ diff --git a/flight/PiOS/inc/pios_settings.h b/flight/PiOS/inc/pios_settings.h index b28b002a1..2336a3a44 100644 --- a/flight/PiOS/inc/pios_settings.h +++ b/flight/PiOS/inc/pios_settings.h @@ -27,7 +27,6 @@ #define PIOS_SETTINGS_H /* Default Values */ -/* GPSUART Default Values */ #define GPS_BAUDRATE 19200 #define TELEM_BAUDRATE 19200 @@ -35,6 +34,9 @@ #define AUXUART_ENABLED 0 #define AUXUART_BAUDRATE 19200 +#define SERVOS_POSITION_MIN 800 +#define SERVOS_POSITION_MAX 2200 + /* Global types */ typedef struct { uint32_t Baudrate; @@ -49,10 +51,16 @@ typedef struct { uint32_t Baudrate; } USARTSettingsTypeDef; +typedef struct { + uint16_t PositionMax; + uint16_t PositionMin; +} ServosSettingsTypeDef; + typedef struct { GPSSettingsTypeDef GPS; TelemSettingsTypeDef Telem; USARTSettingsTypeDef AuxUSART; + ServosSettingsTypeDef Servos; } SettingsTypeDef; /*Global Variables */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 703c129ea..85c586de7 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -56,6 +56,7 @@ #include "pios_usart.h" #include "pios_irq.h" #include "pios_adc.h" +#include "pios_servo.h" /* PIOS Hardware Includes (Common) */ #include "pios_settings.h" diff --git a/flight/doc/SDCard Files/Settings.ini b/flight/doc/SDCard Files/Settings.ini index 65918063c..61e262675 100644 --- a/flight/doc/SDCard Files/Settings.ini +++ b/flight/doc/SDCard Files/Settings.ini @@ -13,3 +13,7 @@ Baudrate = 19200 # !!WARNING!! Enabling this will DISABLE RC input 5 and 6 Enabled = 0 Baudrate = 19200 + +[Servos] +PositionMin = 800 +PositionMax = 2200 \ No newline at end of file