rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-22 Flight/PiOS: added failsafe and recovery "watchdog" timer

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1697 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
sambas 2010-09-20 18:00:31 +00:00 committed by sambas
parent 5fb45f835e
commit 24840b461d
2 changed files with 72 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/PiOS/Boards/STM3210E_OP.h
View File

@ -253,6 +253,15 @@ TIM8 | Servo 5 | Servo 6 | Servo 7 | Servo 8
#define PIOS_PPM_SUPV_IRQ_CHANNEL TIM6_IRQn #define PIOS_PPM_SUPV_IRQ_CHANNEL TIM6_IRQn
#define PIOS_PPM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void) #define PIOS_PPM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
//-------------------------
// SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_SUPV_ENABLED 1
#define PIOS_SPEKTRUM_SUPV_TIMER TIM6
#define PIOS_SPEKTRUM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
#define PIOS_SPEKTRUM_SUPV_HZ 50 // 1/22ms
#define PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL TIM6_IRQn
#define PIOS_SPEKTRUM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
//------------------------- //-------------------------
// Servo outputs // Servo outputs

67
flight/PiOS/STM32F10x/pios_spektrum.c
View File

@ -46,7 +46,8 @@
/* Local Variables, use pios_usart */ /* Local Variables, use pios_usart */
static uint16_t CaptureValue[12]; static uint16_t CaptureValue[12];
static uint8_t prev_byte=0xFF,sync=0,bytecount=0,byte_array[20]={0};
uint8_t sync_of=0;
/** /**
* Initialise the onboard USARTs * Initialise the onboard USARTs
@ -58,6 +59,37 @@ void PIOS_SPEKTRUM_Init(void)
{ {
PIOS_SPEKTRUM_Bind(); PIOS_SPEKTRUM_Bind();
} }
/* spektrum "watchdog" timer */
NVIC_InitTypeDef NVIC_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
/* Enable timer clock */
PIOS_SPEKTRUM_SUPV_TIMER_RCC_FUNC;
/* Configure interrupts */
NVIC_InitStructure.NVIC_IRQChannel = PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Time base configuration */
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = ((1000000 / PIOS_SPEKTRUM_SUPV_HZ) - 1);
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; /* For 1 uS accuracy */
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(PIOS_PPM_SUPV_TIMER, &TIM_TimeBaseStructure);
/* Enable the Update Interrupt Request */
TIM_ITConfig(PIOS_SPEKTRUM_SUPV_TIMER, TIM_IT_Update, ENABLE);
/* Clear update pending flag */
TIM_ClearFlag(TIM6, TIM_FLAG_Update);
/* Enable counter */
TIM_Cmd(PIOS_SPEKTRUM_SUPV_TIMER, ENABLE);
} }
@ -141,7 +173,6 @@ uint8_t PIOS_SPEKTRUM_Bind(void)
*/ */
int32_t PIOS_SPEKTRUM_Decode(uint8_t b) int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
{ {
static uint8_t prev_byte=0xFF,sync=0,bytecount=0,byte_array[20]={0};
static uint16_t channel=0,sync_word=0; static uint16_t channel=0,sync_word=0;
uint8_t channeln=0,frame=0; uint8_t channeln=0,frame=0;
uint16_t data=0; uint16_t data=0;
@ -150,8 +181,9 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
if(sync==0) if(sync==0)
{ {
sync_word=(prev_byte<<8)+b; sync_word=(prev_byte<<8)+b;
if((sync_word&0xCCFE)==0) if(((sync_word&0x00FE)==0) && (bytecount == 2))
{ {
/* sync low byte always 0x01, high byte seems to be random when switching TX on off on, loss counter??? */
if(sync_word&0x01) if(sync_word&0x01)
{ {
sync=1; sync=1;
@ -173,9 +205,10 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
} }
if(bytecount==16) if(bytecount==16)
{ {
//PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff
bytecount=0; bytecount=0;
sync=0; sync=0;
sync_of=0;
} }
prev_byte=b; prev_byte=b;
return 0; return 0;
@ -196,8 +229,34 @@ void SPEKTRUM_IRQHandler(void)
/* Disable TXE interrupt (TXEIE=0) */ /* Disable TXE interrupt (TXEIE=0) */
USART1->CR1 &= ~(1 << 7); USART1->CR1 &= ~(1 << 7);
} }
/* clear "watchdog" timer */
TIM_SetCounter(PIOS_SPEKTRUM_SUPV_TIMER,0);
} }
/**
* This function handles TIM6 global interrupt request.
*/
PIOS_SPEKTRUM_SUPV_IRQ_FUNC
{
/* Clear timer interrupt pending bit */
TIM_ClearITPendingBit(PIOS_SPEKTRUM_SUPV_TIMER, TIM_IT_Update);
/* sync between frames, TODO! DX7SE */
sync=0;
bytecount=0;
prev_byte=0xFF;
sync_of++;
/* watchdog activated */
if(sync_of>1)
{
/* signal lost */
sync_of=0;
for(int i=0;i<12;i++)
CaptureValue[i]=0;
}
}
#endif #endif
/** /**