Subversion Repositories NaviCtrl

Rev

Rev 119 | Rev 121 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 119 Rev 120
Line 59... Line 59...
59 
#include "uart1.h"
 59 
#include "uart1.h"
Line 60... Line 60...
60 
 
 60 
 
61 
#define CR1_OLVL1_MASK    0x0100
 61 
#define CR1_OLVL1_MASK    0x0100
Line -... Line 62...
- 
 
 62 
#define CR1_OLVL2_MASK    0x0200
- 
 
 63 
 
- 
 
 64 
ServoParams_t ServoParams;
62 
#define CR1_OLVL2_MASK    0x0200
 65 
 
63 
 
 66 
#define TIM2_FREQ 625000 // 625kHz,  the same clock like at the FC
64 
// frame len 22.5 ms = 14063 * 1.6 us
 67 
// frame len 22.5 ms = 14063 * 1.6 us
65 
// stop pulse: 0.3 ms = 188 * 1.6 us
 68 
// stop pulse: 0.3 ms = 188 * 1.6 us
66 
// min servo pulse: 0.6 ms =  375 * 1.6 us
 69 
// min servo pulse: 0.6 ms =  375 * 1.6 us
67 
// max servo pulse: 2.4 ms = 1500 * 1.6 us
 - 
 
68 
// resolution: 1500 - 375 = 1125 steps
 - 
 
69 
 
 - 
 
70 
 
 - 
 
71 
ServoParams_t ServoParams;
 70 
// max servo pulse: 2.4 ms = 1500 * 1.6 us
72 
 
 71 
// resolution: 1500 - 375 = 1125 steps
73 
#define PPM_STOPPULSE 188
 72 
#define PPM_STOPPULSE 188
74 
#define PPM_FRAMELEN 14063
 73 
#define PPM_FRAMELEN 14063
75 
//#define PPM_FRAMELEN (1757 * ParamSet.ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel
 74 
//#define PPM_FRAMELEN (1757 * ServoParams.Refresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel
76 
#define MINSERVOPULSE 375
 75 
#define MINSERVOPULSE 375
Line 77... Line 76...
77 
#define MAXSERVOPULSE 1500
 76 
#define MAXSERVOPULSE 1500
Line 90... Line 89...
90 
        if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC1) == SET)
 89 
        if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC1) == SET)
91 
        {
 90 
        {
92 
                if (TIM2->CR1 & CR1_OLVL1_MASK) // start of high pulse
 91 
                if (TIM2->CR1 & CR1_OLVL1_MASK) // start of high pulse
93 
                {
 92 
                {
94 
                        pulselen = MINSERVOPULSE + SERVORANGE/2;
 93 
                        pulselen = MINSERVOPULSE + SERVORANGE/2;
- 
 
 94 
                        // input code for nick servo here
95 
                        LowPulseTime1 = PPM_FRAMELEN - pulselen;
 95 
                        LowPulseTime1 = PPM_FRAMELEN - pulselen;
96 
                        TIM2->CR1 &= ~CR1_OLVL1_MASK; // make next a low pulse
 96 
                        TIM2->CR1 &= ~CR1_OLVL1_MASK; // make next a low pulse
97 
                }
 97 
                }
98 
                else // start of low pulse
 98 
                else // start of low pulse
99 
                {
 99 
                {
Line 107... Line 107...
107 
        if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC2) == SET)
 107 
        if(TIM_GetFlagStatus(TIM2, TIM_FLAG_OC2) == SET)
108 
        {
 108 
        {
109 
                if (TIM2->CR1 & CR1_OLVL2_MASK) // was high pulse
 109 
                if (TIM2->CR1 & CR1_OLVL2_MASK) // was high pulse
110 
                {
 110 
                {
111 
                        pulselen = MINSERVOPULSE + SERVORANGE/2;
 111 
                        pulselen = MINSERVOPULSE + SERVORANGE/2;
- 
 
 112 
                        // input code for roll servo here
112 
                        LowPulseTime2 = PPM_FRAMELEN - pulselen;
 113 
                        LowPulseTime2 = PPM_FRAMELEN - pulselen;
113 
                        TIM2->CR1 &= ~CR1_OLVL2_MASK; // make low pulse
 114 
                        TIM2->CR1 &= ~CR1_OLVL2_MASK; // make next a low pulse
114 
                }
 115 
                }
115 
                else
 116 
                else
116 
                {
 117 
                {
117 
                        pulselen = LowPulseTime2;
 118 
                        pulselen = LowPulseTime2;
118 
                        TIM2->CR1 |= CR1_OLVL2_MASK;
 119 
                        TIM2->CR1 |= CR1_OLVL2_MASK; // make next a high pulse
119 
                }
 120 
                }
120 
                TIM2->OC2R += pulselen;
 121 
                TIM2->OC2R += pulselen;
121 
                TIM_ClearFlag(TIM2, TIM_FLAG_OC2); // clear irq pending bit
 122 
                TIM_ClearFlag(TIM2, TIM_FLAG_OC2); // clear irq pending bit
122 
        }
 123 
        }
Line 131... Line 132...
131 
void TIMER2_Init(void)
 132 
void TIMER2_Init(void)
132 
{
 133 
{
133 
        GPIO_InitTypeDef GPIO_InitStructure;
 134 
        GPIO_InitTypeDef GPIO_InitStructure;
134 
        TIM_InitTypeDef   TIM_InitStructure;
 135 
        TIM_InitTypeDef   TIM_InitStructure;
Line 135... Line 136...
135 
 
 136 
 
136 
        UART1_PutString("Timer2 init...");
 - 
 
137 
 
 - 
 
138 
        #define TIM2_FREQ 625000 // 625kHz
 - 
 
Line 139... Line 137...
139 
 
 137 
        UART1_PutString("\r\n Timer2 init...");
Line 140... Line 138...
140 
 
 138 
 
141 
        SCU_APBPeriphClockConfig(__GPIO6, ENABLE); // Enable the GPIO6 Clock
 139 
        SCU_APBPeriphClockConfig(__GPIO6, ENABLE); // Enable the GPIO6 Clock
Line 157... Line 155...
157 
    GPIO_InitStructure.GPIO_Alternate =         GPIO_OutputAlt2; //TIM2_OCMP2
 155 
    GPIO_InitStructure.GPIO_Alternate =         GPIO_OutputAlt2; //TIM2_OCMP2
158 
    GPIO_Init(GPIO6, &GPIO_InitStructure);
 156 
    GPIO_Init(GPIO6, &GPIO_InitStructure);
Line 159... Line 157...
159 
 
 157 
 
Line -... Line 158...
- 
 
 158 
        SCU_APBPeriphClockConfig(__TIM23, ENABLE);
160 
        SCU_APBPeriphClockConfig(__TIM23, ENABLE);
 159 
 
161 
 
 160 
        TIM_DeInit(TIM2);
162 
        TIM_StructInit(&TIM_InitStructure);
 161 
        TIM_StructInit(&TIM_InitStructure);
163 
        TIM_InitStructure.TIM_Mode = TIM_OCM_CHANNEL_12;
 162 
        TIM_InitStructure.TIM_Mode = TIM_OCM_CHANNEL_12; // Output Compare Channels 1 & 2 Mode
164 
        TIM_InitStructure.TIM_OC1_Modes = TIM_TIMING;
 163 
        TIM_InitStructure.TIM_OC1_Modes = TIM_WAVE;     // OCMP1 pin is dedicated to the OC1 capability of the TIM
165 
        TIM_InitStructure.TIM_OC2_Modes = TIM_TIMING;
 164 
        TIM_InitStructure.TIM_OC2_Modes = TIM_WAVE;     // OCMP2 pin is dedicated to the OC2 capability of the TIM
166 
        TIM_InitStructure.TIM_Clock_Source = TIM_CLK_APB;
 165 
        TIM_InitStructure.TIM_Clock_Source = TIM_CLK_APB; // APB clock source is used
167 
        TIM_InitStructure.TIM_Pulse_Level_1 = TIM_LOW;
 166 
        TIM_InitStructure.TIM_Pulse_Level_1 = TIM_LOW; // output low at OCMP1 pin on compare match
- 
 
 167 
        TIM_InitStructure.TIM_Pulse_Level_2 = TIM_LOW; // output low at OCMP2 pin on compare match
168 
        TIM_InitStructure.TIM_Pulse_Level_2 = TIM_LOW;
 168 
        TIM_InitStructure.TIM_Prescaler = (SCU_GetPCLKFreqValue() * 1000) / TIM2_FREQ;
Line 169... Line 169...
169 
        TIM_InitStructure.TIM_Prescaler = (SCU_GetPCLKFreqValue() * 1000) / TIM2_FREQ;
 169 
 
170 
        TIM_Init(TIM2, &TIM_InitStructure);
 170 
        TIM_Init(TIM2, &TIM_InitStructure);
Line 171... Line 171...
171 
 
 171 
 
172 
        TIM_ITConfig(TIM2, TIM_IT_OC1|TIM_IT_OC2, ENABLE);
 172 
        TIM_ITConfig(TIM2, TIM_IT_OC1|TIM_IT_OC2, ENABLE);  // enable interrupts for the OC 1 & 2
Line 173... Line 173...
173 
        TIM_CounterCmd(TIM2, TIM_START);
 173 
        TIM_CounterCmd(TIM2, TIM_START); // start the timer
Line 184... Line 184...
184 
        ServoParams.RollOffset = 127;
 184 
        ServoParams.RollOffset = 127;
185 
        ServoParams.RollComp = 40;
 185 
        ServoParams.RollComp = 40;
186 
        ServoParams.RollMin = 50;
 186 
        ServoParams.RollMin = 50;
187 
        ServoParams.RollMax = 205;             
 187 
        ServoParams.RollMax = 205;             
Line 188... Line 188...
188 
 
 188 
 
189 
        UART1_PutString("ok\n\r");
 189 
        UART1_PutString("ok");