mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
2f86e4dd4f
PWM and PPM can now coexist in the same load and be selected at boot time via the hwsettings UAVObject. This is basically a complete restructuring of the way the drivers interact with the TIM peripheral in the STM32. As a side effect, the PWM and PPM drivers are now ready to support multiple instances of each. This also provides the first step toward being able to reassign some of the PWM input pins to be servo output pins. Still more work required, but this is a good start.
148 lines
4.2 KiB
C
148 lines
4.2 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
|
* @{
|
|
* @addtogroup PIOS_SERVO RC Servo Functions
|
|
* @brief Code to do set RC servo output
|
|
* @{
|
|
*
|
|
* @file pios_servo.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief RC Servo routines (STM32 dependent)
|
|
* @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
|
|
*/
|
|
|
|
/* Project Includes */
|
|
#include "pios.h"
|
|
#include "pios_servo_priv.h"
|
|
#include "pios_tim_priv.h"
|
|
|
|
/* Private Function Prototypes */
|
|
|
|
static const struct pios_servo_cfg * servo_cfg;
|
|
|
|
/**
|
|
* Initialise Servos
|
|
*/
|
|
int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg)
|
|
{
|
|
uint32_t tim_id;
|
|
if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) {
|
|
return -1;
|
|
}
|
|
|
|
/* Store away the requested configuration */
|
|
servo_cfg = cfg;
|
|
|
|
/* Configure the channels to be in output compare mode */
|
|
for (uint8_t i = 0; i < cfg->num_channels; i++) {
|
|
const struct pios_tim_channel * chan = &cfg->channels[i];
|
|
|
|
/* Set up for output compare function */
|
|
switch(chan->timer_chan) {
|
|
case TIM_Channel_1:
|
|
TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
|
|
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
|
|
break;
|
|
case TIM_Channel_2:
|
|
TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
|
|
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
|
|
break;
|
|
case TIM_Channel_3:
|
|
TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
|
|
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
|
|
break;
|
|
case TIM_Channel_4:
|
|
TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
|
|
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
|
|
break;
|
|
}
|
|
|
|
TIM_ARRPreloadConfig(chan->timer, ENABLE);
|
|
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
|
|
TIM_Cmd(chan->timer, ENABLE);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Set the servo update rate (Max 500Hz)
|
|
* \param[in] array of rates in Hz
|
|
* \param[in] maximum number of banks
|
|
*/
|
|
void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks)
|
|
{
|
|
if (!servo_cfg) {
|
|
return;
|
|
}
|
|
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
|
|
|
|
uint8_t set = 0;
|
|
|
|
for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
|
|
bool new = true;
|
|
const struct pios_tim_channel * chan = &servo_cfg->channels[i];
|
|
|
|
/* See if any previous channels use that same timer */
|
|
for(uint8_t j = 0; (j < i) && new; j++)
|
|
new &= chan->timer != servo_cfg->channels[j].timer;
|
|
|
|
if(new) {
|
|
TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
|
|
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
|
|
set++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Set servo position
|
|
* \param[in] Servo Servo number (0-7)
|
|
* \param[in] Position Servo position in microseconds
|
|
*/
|
|
void PIOS_Servo_Set(uint8_t servo, uint16_t position)
|
|
{
|
|
/* Make sure servo exists */
|
|
if (!servo_cfg || servo >= servo_cfg->num_channels) {
|
|
return;
|
|
}
|
|
|
|
/* Update the position */
|
|
const struct pios_tim_channel * chan = &servo_cfg->channels[servo];
|
|
switch(chan->timer_chan) {
|
|
case TIM_Channel_1:
|
|
TIM_SetCompare1(chan->timer, position);
|
|
break;
|
|
case TIM_Channel_2:
|
|
TIM_SetCompare2(chan->timer, position);
|
|
break;
|
|
case TIM_Channel_3:
|
|
TIM_SetCompare3(chan->timer, position);
|
|
break;
|
|
case TIM_Channel_4:
|
|
TIM_SetCompare4(chan->timer, position);
|
|
break;
|
|
}
|
|
}
|