mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Bring F4 PiOS up to date with some of our timer conventions
This commit is contained in:
parent
48d2e038bb
commit
0931a518d4
@ -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);
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -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
|
||||
}
|
||||
|
||||
|
||||
|
@ -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. */
|
||||
|
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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[];
|
||||
|
@ -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);
|
||||
|
@ -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_* */
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
x
Reference in New Issue
Block a user