rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Bring F4 PiOS up to date with some of our timer conventions

Browse Source
This commit is contained in:
James Cotton 2011-11-01 04:39:51 -05:00
parent 48d2e038bb
commit 0931a518d4
14 changed files with 562 additions and 417 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/PiOS/STM32F4xx/Libraries/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_tim.h
View File

@ -1018,7 +1018,7 @@ typedef struct
/* TimeBase management ********************************************************/
void TIM_DeInit(TIM_TypeDef* TIMx);
void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);
void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);
void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);
void TIM_PrescalerConfig(TIM_TypeDef* TIMx, uint16_t Prescaler, uint16_t TIM_PSCReloadMode);
void TIM_CounterModeConfig(TIM_TypeDef* TIMx, uint16_t TIM_CounterMode);
@ -1034,10 +1034,10 @@ void TIM_SetClockDivision(TIM_TypeDef* TIMx, uint16_t TIM_CKD);
void TIM_Cmd(TIM_TypeDef* TIMx, FunctionalState NewState);
/* Output Compare management **************************************************/
void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_OCStructInit(TIM_OCInitTypeDef* TIM_OCInitStruct);
void TIM_SelectOCxM(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);
void TIM_SetCompare1(TIM_TypeDef* TIMx, uint32_t Compare1);

10
flight/PiOS/STM32F4xx/Libraries/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.c
View File

@ -279,7 +279,7 @@ void TIM_DeInit(TIM_TypeDef* TIMx)
* that contains the configuration information for the specified TIM peripheral.
* @retval None
*/
void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct)
void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct)
{
uint16_t tmpcr1 = 0;
@ -662,7 +662,7 @@ void TIM_Cmd(TIM_TypeDef* TIMx, FunctionalState NewState)
* the configuration information for the specified TIM peripheral.
* @retval None
*/
void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
{
uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
@ -743,7 +743,7 @@ void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
* the configuration information for the specified TIM peripheral.
* @retval None
*/
void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
{
uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
@ -824,7 +824,7 @@ void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
* the configuration information for the specified TIM peripheral.
* @retval None
*/
void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
{
uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
@ -904,7 +904,7 @@ void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
* the configuration information for the specified TIM peripheral.
* @retval None
*/
void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
{
uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;

3
flight/PiOS/STM32F4xx/pios_bmp085.c
View File

@ -35,9 +35,6 @@
#include "pios.h"
#if defined(PIOS_INCLUDE_BMP085)
#if !defined(PIOS_INCLUDE_EXTI)
#error PIOS_EXTI Must be included in the project!
#endif /* PIOS_INCLUDE_EXTI */
/* Glocal Variables */
ConversionTypeTypeDef CurrentRead;

123
flight/PiOS/STM32F4xx/pios_buzzer.c
View File

@ -1,123 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SERVO RC Servo Functions
* @brief Code to do set RC servo output
* @{
*
* @file pios_buzzer.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Buzzer 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_buzzer_priv.h"
/* Private Function Prototypes */
uint16_t current_frequency;
uint16_t intervals[12] = { 57334, 54116, 51079, 48212, 45506, 42952, 40541, 38266, 36118, 34091, 32178, 30372 };
/**
* Initialise Servos
*/
void PIOS_Buzzer_Init(void)
{
#if defined(PIOS_INCLUDE_BUZZER)
GPIO_InitTypeDef GPIO_InitStructure = pios_buzzer_cfg.gpio_init;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_buzzer_cfg.tim_base_init;
TIM_OCInitTypeDef TIM_OCInitStructure = pios_buzzer_cfg.tim_oc_init;
/* Enable GPIO */
GPIO_Init(pios_buzzer_cfg.port, &GPIO_InitStructure);
GPIO_PinAFConfig(pios_buzzer_cfg.port, pios_buzzer_cfg.pin_source, pios_buzzer_cfg.af);
/* Enable time base */
TIM_TimeBaseInit(pios_buzzer_cfg.timer, &TIM_TimeBaseStructure);
/* Set up for output compare function */
switch(pios_buzzer_cfg.channel) {
case TIM_Channel_1:
TIM_OC1Init(pios_buzzer_cfg.timer, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(pios_buzzer_cfg.timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(pios_buzzer_cfg.timer, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(pios_buzzer_cfg.timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(pios_buzzer_cfg.timer, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(pios_buzzer_cfg.timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(pios_buzzer_cfg.timer, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(pios_buzzer_cfg.timer, TIM_OCPreload_Enable);
break;
}
TIM_ARRPreloadConfig(pios_buzzer_cfg.timer, ENABLE);
#endif // PIOS_INCLUDE_BUZZER
}
void PIOS_Buzzer_Ctrl(uint8_t enable)
{
if (enable)
{
TIM_Cmd(pios_buzzer_cfg.timer, ENABLE);
}
else
{
TIM_Cmd(pios_buzzer_cfg.timer, DISABLE);
}
}
/**
* Set the PWM freq
* \param[in] piano note in numbers (MIDI note numbers)
*/
void PIOS_Buzzer_SetNote(uint8_t note)
{
#if defined(PIOS_INCLUDE_BUZZER)
uint16_t tim_interval = intervals[note%12] / (1 << note/12);
TIM_SetAutoreload(pios_buzzer_cfg.timer, tim_interval);
switch(pios_buzzer_cfg.channel) {
case TIM_Channel_1:
TIM_SetCompare1(pios_buzzer_cfg.timer, tim_interval/2);
break;
case TIM_Channel_2:
TIM_SetCompare2(pios_buzzer_cfg.timer, tim_interval/2);
break;
case TIM_Channel_3:
TIM_SetCompare3(pios_buzzer_cfg.timer, tim_interval/2);
break;
case TIM_Channel_4:
TIM_SetCompare4(pios_buzzer_cfg.timer, tim_interval/2);
break;
}
#endif // PIOS_INCLUDE_BUZZER
}

84
flight/PiOS/STM32F4xx/pios_debug.c
View File

@ -34,44 +34,118 @@
// Global variables
const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED";
/* Private Function Prototypes */
#ifdef PIOS_ENABLE_DEBUG_PINS
static const struct pios_tim_channel * debug_channels;
static uint8_t debug_num_channels;
#endif /* PIOS_ENABLE_DEBUG_PINS */
/**
* Initialise Debug-features
*/
void PIOS_DEBUG_Init(void)
void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
PIOS_Assert(channels);
PIOS_Assert(num_channels);
/* Store away the GPIOs we've been given */
debug_channels = channels;
debug_num_channels = num_channels;
/* Configure the GPIOs we've been given */
for (uint8_t i = 0; i < num_channels; i++) {
const struct pios_tim_channel * chan = &channels[i];
// Initialise pins as standard output pins
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = chan->init->GPIO_Pin;
/* Initialize the GPIO */
GPIO_Init(chan->init->port, &GPIO_InitStructure);
/* Set the pin low */
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
}
#endif // PIOS_ENABLE_DEBUG_PINS
}
/**
* Set debug-pin high
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinHigh(uint8_t Pin)
void PIOS_DEBUG_PinHigh(uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
return;
}
const struct pios_tim_channel * chan = &debug_channels[pin];
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_Set);
#endif // PIOS_ENABLE_DEBUG_PINS
}
/**
* Set debug-pin low
* \param pin 0 for S1 output
*/
void PIOS_DEBUG_PinLow(uint8_t Pin)
void PIOS_DEBUG_PinLow(uint8_t pin)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels || pin >= debug_num_channels) {
return;
}
const struct pios_tim_channel * chan = &debug_channels[pin];
GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue8Bit(uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
return;
}
uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6);
uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4));
PIOS_IRQ_Disable();
/*
* This is sketchy since it assumes a particular ordering
* and bitwise layout of the channels provided to the debug code.
*/
debug_channels[0].init.port->BSRR = bsrr_l;
debug_channels[4].init.port->BSRR = bsrr_h;
PIOS_IRQ_Enable();
#endif // PIOS_ENABLE_DEBUG_PINS
}
void PIOS_DEBUG_PinValue4BitL(uint8_t value)
{
#ifdef PIOS_ENABLE_DEBUG_PINS
if (!debug_channels) {
return;
}
/*
* This is sketchy since it assumes a particular ordering
* and bitwise layout of the channels provided to the debug code.
*/
uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6);
debug_channels[0].init.port->BSRR = bsrr_l;
#endif // PIOS_ENABLE_DEBUG_PINS
}

122
flight/PiOS/STM32F4xx/pios_i2c.c
View File

@ -3,7 +3,7 @@
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_I2C I2C Functions
* @brief STM32F4xx Hardware dependent I2C functionality
* @brief STM32F2xx Hardware dependent I2C functionality
* @{
*
* @file pios_i2c.c
@ -132,6 +132,7 @@ static bool i2c_adapter_wait_for_stopped(struct pios_i2c_adapter *i2c_adapter);
static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter);
static void i2c_adapter_log_fault(enum pios_i2c_error_type type);
static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter);
const static struct i2c_adapter_transition i2c_adapter_transitions[I2C_STATE_NUM_STATES] = {
[I2C_STATE_FSM_FAULT] = {
@ -398,6 +399,9 @@ static void go_stopping(struct pios_i2c_adapter *i2c_adapter)
}
portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */
#endif /* USE_FREERTOS */
if(i2c_adapter->callback)
i2c_adapter_callback_handler(i2c_adapter);
}
static void go_stopped(struct pios_i2c_adapter *i2c_adapter)
@ -770,6 +774,47 @@ static bool i2c_adapter_fsm_terminated(struct pios_i2c_adapter *i2c_adapter)
}
}
uint32_t i2c_cb_count = 0;
static bool i2c_adapter_callback_handler(struct pios_i2c_adapter * i2c_adapter)
{
bool semaphore_success = true;
/* Wait for the transfer to complete */
#ifdef USE_FREERTOS
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
xSemaphoreGive(i2c_adapter->sem_ready);
#endif /* USE_FREERTOS */
/* Spin waiting for the transfer to finish */
while (!i2c_adapter_fsm_terminated(i2c_adapter)) ;
if (i2c_adapter_wait_for_stopped(i2c_adapter)) {
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_STOPPED);
} else {
i2c_adapter_fsm_init(i2c_adapter);
}
// Execute user supplied function
i2c_adapter->callback();
i2c_cb_count++;
#ifdef USE_FREERTOS
/* Unlock the bus */
xSemaphoreGive(i2c_adapter->sem_busy);
if(!semaphore_success)
i2c_timeout_counter++;
#else
PIOS_IRQ_Disable();
i2c_adapter->busy = 0;
PIOS_IRQ_Enable();
#endif /* USE_FREERTOS */
return (!i2c_adapter->bus_error) && semaphore_success;
}
/**
* Logs the last N state transitions and N IRQ events due to
* an error condition
@ -889,6 +934,8 @@ int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg
*/
vSemaphoreCreateBinary(i2c_adapter->sem_ready);
i2c_adapter->sem_busy = xSemaphoreCreateMutex();
#else
i2c_adapter->busy = 0;
#endif // USE_FREERTOS
/* Initialize the state machine */
@ -907,11 +954,8 @@ out_fail:
return(-1);
}
uint32_t transfers = 0;
uint32_t transfers_successful = 0;
bool PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns)
int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns)
{
transfers++;
struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
@ -927,6 +971,14 @@ bool PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], ui
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdTRUE);
#else
PIOS_IRQ_Disable();
if(i2c_adapter->busy) {
PIOS_IRQ_Enable();
return -2;
}
i2c_adapter->busy = 1;
PIOS_IRQ_Enable();
#endif /* USE_FREERTOS */
PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED);
@ -940,6 +992,7 @@ bool PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], ui
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
i2c_adapter->callback = NULL;
i2c_adapter->bus_error = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
@ -963,12 +1016,59 @@ bool PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], ui
xSemaphoreGive(i2c_adapter->sem_busy);
if(!semaphore_success)
i2c_timeout_counter++;
#else
PIOS_IRQ_Disable();
i2c_adapter->busy = 0;
PIOS_IRQ_Enable();
#endif /* USE_FREERTOS */
transfers_successful+= (!i2c_adapter->bus_error) && semaphore_success;
return (!i2c_adapter->bus_error) && semaphore_success;
return !semaphore_success ? -2 :
i2c_adapter->bus_error ? -1 :
0;
}
int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns, void *callback)
{
struct pios_i2c_adapter * i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
bool valid = PIOS_I2C_validate(i2c_adapter);
PIOS_Assert(valid)
PIOS_Assert(callback);
PIOS_DEBUG_Assert(txn_list);
PIOS_DEBUG_Assert(num_txns);
bool semaphore_success = true;
#ifdef USE_FREERTOS
/* Lock the bus */
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdTRUE);
#else
if(i2c_adapter->busy) {
PIOS_IRQ_Enable();
return -2;
}
#endif /* USE_FREERTOS */
PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED);
i2c_adapter->first_txn = &txn_list[0];
i2c_adapter->last_txn = &txn_list[num_txns - 1];
i2c_adapter->active_txn = i2c_adapter->first_txn;
#ifdef USE_FREERTOS
/* Make sure the done/ready semaphore is consumed before we start */
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_ready, timeout) == pdTRUE);
#endif
i2c_adapter->callback = callback;
i2c_adapter->bus_error = false;
i2c_adapter_inject_event(i2c_adapter, I2C_EVENT_START);
return !semaphore_success ? -2 : 0;
}
void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
{
@ -988,7 +1088,13 @@ void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
#define EVENT_MASK 0x000700FF
event &= EVENT_MASK;
// This is very poor and inconsistent practice with the FSM since no other
// throw event depends on the current state. However when accelerated (-Os)
// we definitely catch this event twice and there is no clean way to do deal
// with that in the FMS short of a special state for it
if(i2c_adapter->curr_state == I2C_STATE_STARTING && event == 0x70084)
return;
switch (event) { /* Mask out all the bits we don't care about */
case (I2C_EVENT_MASTER_MODE_SELECT | 0x40):
/* Unexplained event: EV5 + RxNE : Extraneous Rx. Probably a late NACK from previous read. */

430
flight/PiOS/STM32F4xx/pios_ppm.c
View File

@ -28,84 +28,175 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/*
* @note This is a cleaned up and cut down version of the F1xx PPM input
* module that uses a FreeRTOS task to handle the supervisor.
*
* @todo It would be cheaper (less stack involved) to use a timer, or
* even better, a callout (but that would require implementing callouts).
*/
/* Project Includes */
#include <pios.h>
#include <pios_ppm_priv.h>
#include <timers.h>
#include "pios.h"
#include "pios_ppm_priv.h"
#ifndef PIOS_INCLUDE_FREERTOS
# error PPM input requires FreeRTOS
#else
# if !configUSE_TIMERS
# error PPM input requires FreeRTOS configUSE_TIMERS
# endif
#endif
#if defined(PIOS_INCLUDE_PPM)
/* Provide a RCVR driver */
static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel);
static int32_t PIOS_PPM_Get_RSSI(uint32_t rcvr_id);
const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
.read = PIOS_PPM_Get,
.rssi = PIOS_PPM_Get_RSSI
};
#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4
#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS
#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames
#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds
#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
/* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure;
static uint8_t PulseIndex;
static uint32_t PreviousValue;
static uint32_t CurrentValue;
static uint32_t CapturedValue;
static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS];
static uint32_t CapCounter[PIOS_PPM_NUM_INPUTS];
static uint16_t TimerCounter;
static uint8_t RSSI = 0;
static void PIOS_PPM_Supervisor(uint32_t ppm_id);
static uint8_t SupervisorState = 0;
static uint32_t CapCounterPrev[PIOS_PPM_NUM_INPUTS];
enum pios_ppm_dev_magic {
PIOS_PPM_DEV_MAGIC = 0xee014d8b,
};
static xTimerHandle ppmSupvTimer;
static void ppmSupvCallback(xTimerHandle xTimer);
struct pios_ppm_dev {
enum pios_ppm_dev_magic magic;
const struct pios_ppm_cfg * cfg;
/**
* Do PPM input initialisation.
*/
void PIOS_PPM_Init(void)
uint8_t PulseIndex;
uint32_t PreviousTime;
uint32_t CurrentTime;
uint32_t DeltaTime;
uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS];
uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS];
uint32_t LargeCounter;
int8_t NumChannels;
int8_t NumChannelsPrevFrame;
uint8_t NumChannelCounter;
uint8_t supv_timer;
bool Tracking;
bool Fresh;
};
static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev)
{
/* Enable timer interrupts */
NVIC_Init((NVIC_InitTypeDef *)&pios_ppm_cfg.irq.init);
return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC);
}
/* Configure input pins */
GPIO_Init(pios_ppm_cfg.port, (GPIO_InitTypeDef *)&pios_ppm_cfg.gpio_init);
GPIO_PinAFConfig(pios_ppm_cfg.port, __builtin_ctz(pios_ppm_cfg.gpio_init.GPIO_Pin), pios_ppm_cfg.remap);
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_ppm_dev * PIOS_PPM_alloc(void)
{
struct pios_ppm_dev * ppm_dev;
/* Configure timer for input capture */
TIM_ICInit(pios_ppm_cfg.timer, (TIM_ICInitTypeDef *)&pios_ppm_cfg.tim_ic_init);
ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev));
if (!ppm_dev) return(NULL);
/* Configure timer clocks */
TIM_InternalClockConfig(pios_ppm_cfg.timer);
TIM_TimeBaseInit(pios_ppm_cfg.timer, (TIM_TimeBaseInitTypeDef *)&pios_ppm_cfg.tim_base_init);
ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
return(ppm_dev);
}
#else
static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS];
static uint8_t pios_ppm_num_devs;
static struct pios_ppm_dev * PIOS_PPM_alloc(void)
{
struct pios_ppm_dev * ppm_dev;
/* Enable the Capture Compare Interrupt Request */
TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr | TIM_IT_Update, ENABLE);
/* Enable timer */
TIM_Cmd(pios_ppm_cfg.timer, ENABLE);
/* register the supervisor timer callout at 25Hz */
ppmSupvTimer = xTimerCreate((signed char *)"ppmSupv", configTICK_RATE_HZ / 25, pdTRUE, NULL, ppmSupvCallback);
if(xTimerStart(ppmSupvTimer, 0 ) != pdPASS)
{
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "\nPPM TIMER START FAILED!\r\n", (ppmSupvTimer == NULL));
if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) {
return (NULL);
}
ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++];
ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
return (ppm_dev);
}
#endif
static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
const static struct pios_tim_callbacks tim_callbacks = {
.overflow = PIOS_PPM_tim_overflow_cb,
.edge = PIOS_PPM_tim_edge_cb,
};
extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
{
PIOS_DEBUG_Assert(ppm_id);
PIOS_DEBUG_Assert(cfg);
struct pios_ppm_dev * ppm_dev;
ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc();
if (!ppm_dev) goto out_fail;
/* Bind the configuration to the device instance */
ppm_dev->cfg = cfg;
/* Set up the state variables */
ppm_dev->PulseIndex = 0;
ppm_dev->PreviousTime = 0;
ppm_dev->CurrentTime = 0;
ppm_dev->DeltaTime = 0;
ppm_dev->LargeCounter = 0;
ppm_dev->NumChannels = -1;
ppm_dev->NumChannelsPrevFrame = -1;
ppm_dev->NumChannelCounter = 0;
ppm_dev->Tracking = false;
ppm_dev->Fresh = false;
for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
/* Flush counter variables */
ppm_dev->CaptureValue[i] = 0;
ppm_dev->CaptureValueNewFrame[i] = 0;
}
uint32_t tim_id;
if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) {
return -1;
}
/* Configure the channels to be in capture/compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i];
/* Configure timer for input capture */
TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
TIM_ICInit(chan->timer, &TIM_ICInitStructure);
/* Enable the Capture Compare Interrupt Request */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_2:
TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_3:
TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE);
break;
case TIM_Channel_4:
TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE);
break;
}
}
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
PIOS_DEBUG_Assert(0);
}
*ppm_id = (uint32_t)ppm_dev;
return(0);
out_fail:
return(-1);
}
/**
@ -116,127 +207,150 @@ void PIOS_PPM_Init(void)
*/
static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
{
/* Return error if channel not available */
if (channel >= PIOS_PPM_NUM_INPUTS) {
return -1;
}
return CaptureValue[channel];
}
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id;
/**
* Get the receive signal strength
* \param[in] Channel Number of the channel desired
* \output 0 No signal
* \output >0-255 signal quality
*/
static int32_t PIOS_PPM_Get_RSSI(uint32_t rcvr_id)
{
return RSSI;
}
/**
* Handle TIMx global interrupt request
* Some work and testing still needed, need to detect start of frame and decode pulses
*
*/
void PIOS_PPM_irq_handler(void)
{
if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) {
TimerCounter+=pios_ppm_cfg.timer->ARR;
TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update);
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) != SET) {
return;
}
if (!PIOS_PPM_validate(ppm_dev)) {
/* Invalid device specified */
return PIOS_RCVR_INVALID;
}
if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
/* Channel out of range */
return PIOS_RCVR_INVALID;
}
return ppm_dev->CaptureValue[channel];
}
/* Do this as it's more efficient */
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) {
PreviousValue = CurrentValue;
switch((int32_t) pios_ppm_cfg.ccr) {
case (int32_t)TIM_IT_CC1:
CurrentValue = TIM_GetCapture1(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC2:
CurrentValue = TIM_GetCapture2(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC3:
CurrentValue = TIM_GetCapture3(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC4:
CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer);
break;
}
CurrentValue+=TimerCounter;
if(CurrentValue > 0xFFFF) {
CurrentValue-=0xFFFF;
}
static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
{
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
/* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
if (!PIOS_PPM_validate(ppm_dev)) {
/* Invalid device specified */
return;
}
/* Capture computation */
if (CurrentValue > PreviousValue) {
CapturedValue = (CurrentValue - PreviousValue);
ppm_dev->LargeCounter += count;
return;
}
static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
{
/* Recover our device context */
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
if (!PIOS_PPM_validate(ppm_dev)) {
/* Invalid device specified */
return;
}
if (chan_idx >= ppm_dev->cfg->num_channels) {
/* Channel out of range */
return;
}
/* Shift the last measurement out */
ppm_dev->PreviousTime = ppm_dev->CurrentTime;
/* Grab the new count */
ppm_dev->CurrentTime = count;
/* Convert to 32-bit timer result */
ppm_dev->CurrentTime += ppm_dev->LargeCounter;
/* Capture computation */
ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime;
ppm_dev->PreviousTime = ppm_dev->CurrentTime;
/* Sync pulse detection */
if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) {
if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame
&& ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS
&& ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS)
{
/* If we see n simultaneous frames of the same
number of channels we save it as our frame size */
if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT)
ppm_dev->NumChannelCounter++;
else
ppm_dev->NumChannels = ppm_dev->PulseIndex;
} else {
CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue);
ppm_dev->NumChannelCounter = 0;
}
/* sync pulse */
if (CapturedValue > 8000) {
PulseIndex = 0;
/* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */
} else if (CapturedValue > 750 && CapturedValue < 2500) {
if (PulseIndex < PIOS_PPM_NUM_INPUTS) {
CaptureValue[PulseIndex] = CapturedValue;
CapCounter[PulseIndex]++;
PulseIndex++;
/* Check if the last frame was well formed */
if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
/* The last frame was well formed */
for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
}
for (uint32_t i = ppm_dev->NumChannels;
i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
}
}
}
}
static void
ppmSupvCallback(xTimerHandle xTimer)
{
for (;;) {
/* we should receive a PPM frame at least once every 80 ms (detection takes up to 160 ms) */
vTaskDelay(80 / portTICK_RATE_MS);
ppm_dev->Fresh = true;
ppm_dev->Tracking = true;
ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex;
ppm_dev->PulseIndex = 0;
/* Simple state machine */
if (SupervisorState == 0) {
/* Save this states values */
for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounterPrev[i] = CapCounter[i];
}
/* We rely on the supervisor to set CaptureValue to invalid
if no valid frame is found otherwise we ride over it */
/* Move to next state */
SupervisorState = 1;
} else if (ppm_dev->Tracking) {
/* Valid pulse duration 0.75 to 2.5 ms*/
if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US
&& ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US
&& ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) {
ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime;
ppm_dev->PulseIndex++;
} else {
/* See what channels have been updated */
bool signalLostCount = 0; // If no channel was triggered, we lost signal
for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
if (CapCounter[i] == CapCounterPrev[i]) {
CaptureValue[i] = 0;
signalLostCount++;
}
/* Not a valid pulse duration */
ppm_dev->Tracking = false;
for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
}
// If more than 33% of the configured channels could not be captured
// assume an error state. This allows to use 4 channel systems
// to be used with 8 configured input channels
if (signalLostCount > PIOS_PPM_NUM_INPUTS/3) {
RSSI = 0;
} else {
RSSI = 255;
}
/* Move to next state */
SupervisorState = 0;
}
}
}
static void PIOS_PPM_Supervisor(uint32_t ppm_id) {
/* Recover our device context */
struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id;
if (!PIOS_PPM_validate(ppm_dev)) {
/* Invalid device specified */
return;
}
/*
* RTC runs at 625Hz so divide down the base rate so
* that this loop runs at 25Hz.
*/
if(++(ppm_dev->supv_timer) < 25) {
return;
}
ppm_dev->supv_timer = 0;
if (!ppm_dev->Fresh) {
ppm_dev->Tracking = false;
for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
}
}
ppm_dev->Fresh = false;
}
#endif
/**
* @}
* @}

182
flight/PiOS/STM32F4xx/pios_servo.c
View File

@ -6,98 +6,80 @@
* @brief Code to do set RC servo output
* @{
*
* @file pios_servo.c
* @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
/*
* 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
*
* 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.,
*
* 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 */
uint16_t servo_positions[8];
static const struct pios_servo_cfg * servo_cfg;
/**
* Initialise Servos
*/
void PIOS_Servo_Init(void)
int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg)
{
#ifndef PIOS_ENABLE_DEBUG_PINS
#if defined(PIOS_INCLUDE_SERVO)
RCC_ClocksTypeDef clocks;
RCC_GetClocksFreq(&clocks);
for (uint8_t i = 0; i < pios_servo_cfg.num_channels; i++) {
GPIO_InitTypeDef GPIO_InitStructure = pios_servo_cfg.gpio_init;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init;
TIM_OCInitTypeDef TIM_OCInitStructure = pios_servo_cfg.tim_oc_init;
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];
struct pios_servo_channel channel = pios_servo_cfg.channels[i];
/* Enable GPIO */
GPIO_InitStructure.GPIO_Pin = channel.pin;
GPIO_Init(channel.port, &GPIO_InitStructure);
/* Enable time base */
TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure);
/* Set up for output compare function */
switch(channel.channel) {
switch(chan->timer_chan) {
case TIM_Channel_1:
channel.timer->PSC = ((clocks.PCLK2_Frequency * 2) / 1000000) - 1;
TIM_OC1Init(channel.timer, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(channel.timer, TIM_OCPreload_Enable);
TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
channel.timer->PSC = ((clocks.PCLK1_Frequency * 2) / 1000000) - 1;
TIM_OC2Init(channel.timer, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(channel.timer, TIM_OCPreload_Enable);
TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
channel.timer->PSC = ((clocks.PCLK1_Frequency * 2) / 1000000) - 1;
TIM_OC3Init(channel.timer, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(channel.timer, TIM_OCPreload_Enable);
TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
channel.timer->PSC = ((clocks.PCLK1_Frequency * 2) / 1000000) - 1;
TIM_OC4Init(channel.timer, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(channel.timer, TIM_OCPreload_Enable);
TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
TIM_ARRPreloadConfig(channel.timer, ENABLE);
TIM_CtrlPWMOutputs(channel.timer, ENABLE);
TIM_Cmd(channel.timer, ENABLE);
}
if(pios_servo_cfg.remap) {
/* Warning, I don't think this will work for multiple remaps at once */
GPIO_PinRemapConfig(pios_servo_cfg.remap, ENABLE);
}
#endif // PIOS_INCLUDE_SERVO
#endif // PIOS_ENABLE_DEBUG_PINS
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, ENABLE);
}
return 0;
}
/**
@ -107,65 +89,59 @@ void PIOS_Servo_Init(void)
*/
void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks)
{
#ifndef PIOS_ENABLE_DEBUG_PINS
#if defined(PIOS_INCLUDE_SERVO)
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init;
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 < pios_servo_cfg.num_channels) && (set < banks); i++) {
for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
bool new = true;
struct pios_servo_channel channel = pios_servo_cfg.channels[i];
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 &= channel.timer != pios_servo_cfg.channels[j].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(channel.timer, &TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
set++;
}
}
#endif // PIOS_INCLUDE_SERVO
#endif // PIOS_ENABLE_DEBUG_PINS
}
/**
* Set servo position
* \param[in] Servo Servo number (0-7)
* \param[in] Position Servo position in milliseconds
* \param[in] Position Servo position in microseconds
*/
void PIOS_Servo_Set(uint8_t Servo, uint16_t Position)
void PIOS_Servo_Set(uint8_t servo, uint16_t position)
{
#ifndef PIOS_ENABLE_DEBUG_PINS
#if defined(PIOS_INCLUDE_SERVO)
/* Make sure servo exists */
if (Servo < pios_servo_cfg.num_channels && Servo >= 0) {
/* Update the position */
switch(pios_servo_cfg.channels[Servo].channel) {
case TIM_Channel_1:
servo_positions[Servo] = Position;
TIM_SetCompare1(pios_servo_cfg.channels[Servo].timer, Position);
break;
case TIM_Channel_2:
servo_positions[Servo] = Position;
TIM_SetCompare2(pios_servo_cfg.channels[Servo].timer, Position);
break;
case TIM_Channel_3:
servo_positions[Servo] = Position;
TIM_SetCompare3(pios_servo_cfg.channels[Servo].timer, Position);
break;
case TIM_Channel_4:
servo_positions[Servo] = Position;
TIM_SetCompare4(pios_servo_cfg.channels[Servo].timer, Position);
break;
}
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;
}
#endif // PIOS_INCLUDE_SERVO
#endif // PIOS_ENABLE_DEBUG_PINS
}

3
flight/PiOS/STM32F4xx/pios_sys.c
View File

@ -60,9 +60,6 @@ void PIOS_SYS_Init(void)
PIOS_DELAY_Init();
PIOS_DELAY_WaitmS(500); /* XXX wait for the OpenOCD DCC stuff to get its act together */
/* Debug services */
PIOS_DEBUG_Init();
/*
* Turn on all the peripheral clocks.
* Micromanaging clocks makes no sense given the power situation in the system, so

2
flight/PiOS/inc/pios_adc_priv.h
View File

@ -52,9 +52,11 @@ struct pios_adc_dev {
volatile uint8_t adc_oversample;
uint8_t dma_block_size;
uint16_t dma_half_buffer_size;
#if defined(PIOS_INCLUDE_ADC)
int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4)));
volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used
float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4)));
#endif
};
extern struct pios_adc_dev pios_adc_devs[];

2
flight/PiOS/inc/pios_spi.h
View File

@ -45,7 +45,7 @@ typedef enum {
/* Public Functions */
extern int32_t PIOS_SPI_SetClockSpeed(uint32_t spi_id, SPIPrescalerTypeDef spi_prescaler);
#ifdef STM32F2XX /* XXX harmonise these */
#if defined(STM32F2XX) || defined(STM32F4XX) /* XXX harmonise these */
extern int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint32_t slave_id, uint8_t pin_value);
#else
extern int32_t PIOS_SPI_RC_PinSet(uint32_t spi_id, uint8_t pin_value);

2
flight/PiOS/inc/pios_spi_priv.h
View File

@ -36,7 +36,7 @@
#include <pios_stm32.h>
/* XXX these two should be reconciled - separate for now to avoid breaking other targets */
#ifdef STM32F2XX
#if defined(STM32F2XX) || defined(STM32F4XX)
struct pios_spi_cfg {
SPI_TypeDef *regs;
uint32_t remap; /* GPIO_Remap_* or GPIO_AF_* */

2
flight/PiOS/inc/pios_stm32.h
View File

@ -38,7 +38,7 @@ struct stm32_irq {
};
struct stm32_dma_chan {
#if defined(STM32F2XX)
#if defined(STM32F2XX) || defined(STM32F4XX)
DMA_Stream_TypeDef *channel;
#else
DMA_Channel_TypeDef *channel;

4
flight/PiOS/pios.h
View File

@ -48,7 +48,9 @@
#include "pios_config.h"
/* STM32 Std Perf Lib */
#if defined(STM32F2XX)
#if defined(STM32F4XX)
# include <stm32f4xx.h>
#elif defined(STM32F2XX)
#include <stm32f2xx.h>
#include <stm32f2xx_syscfg.h>
#else