Added pios_servo. Straight from ST examples.

Updated printf-stdarg to use debug port.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@53 ebee16cc-31ac-478f-84a7-5cbb03baadba

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

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);
 }
 
 /**

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);
 
 }
 

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;
+        }
+    }
+}

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
 
 //-------------------------

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 */

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 */

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"

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