Subversion Repositories FlightCtrl

Rev

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

Rev 1400 Rev 1568
Line -... Line 1...
- 
 
 1 
 
1 
#include "main.h"
 2 
#include "main.h"
2 
#include "spectrum.h"
 3 
#include "spectrum.h"
- 
 
 4 
#include "heli.h"
Line 3... Line 5...
3 
 
 5 
 
4 
volatile unsigned int CountMilliseconds = 0;
 6 
volatile unsigned int CountMilliseconds = 0;
5 
volatile static unsigned int tim_main;
 7 
volatile static unsigned int tim_main;
6 
volatile unsigned char UpdateMotor = 0;
 8 
volatile unsigned char UpdateMotor = 0;
7 
volatile unsigned int cntKompass = 0;
 9 
volatile unsigned int cntKompass = 0;
8 
volatile unsigned int beeptime = 0;
 10 
volatile unsigned int beeptime = 0;
Line 9... Line 11...
9 
volatile unsigned char SendSPI = 0, ServoActive = 0;
 11 
volatile unsigned char SendSPI = 0;
Line 10... Line 12...
10 
 
 12 
 
11 
unsigned int BeepMuster = 0xffff;
 13 
unsigned int BeepMuster = 0xffff;
Line 29... Line 31...
29 
SIGNAL (SIG_OVERFLOW0)    // 9,7kHz
 31 
SIGNAL (SIG_OVERFLOW0)    // 9,7kHz
30 
{
 32 
{
31 
    static unsigned char cnt_1ms = 1,cnt = 0;
 33 
    static unsigned char cnt_1ms = 1,cnt = 0;
32 
    unsigned char pieper_ein = 0;
 34 
    unsigned char pieper_ein = 0;
33 
   if(SendSPI) SendSPI--;
 35 
   if(SendSPI) SendSPI--;
34 
   if(SpektrumTimer) SpektrumTimer--;
 36 
   if(SpektrumTimer) SpektrumTimer--;
Line 35... Line 37...
35 
 
 37 
 
36 
   if(!cnt--)
 38 
   if(!cnt--)
37 
    {
 39 
    {
38 
     cnt = 9;
 40 
     cnt = 9;
Line 132... Line 134...
132 
{
 134 
{
133 
        uint8_t sreg = SREG;
 135 
        uint8_t sreg = SREG;
Line 134... Line 136...
134 
 
 136 
 
135 
        // disable all interrupts before reconfiguration
 137 
        // disable all interrupts before reconfiguration
136 
        cli();
 138 
        cli();
137 
       
 139 
 
Line 138... Line 140...
138 
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
 140 
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
139 
 
 141 
 
140 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
 142 
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
Line 214... Line 216...
214 
 
 216 
 
215 
        #define MULTIPLYER 4
 217 
        #define MULTIPLYER 4
216 
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
 218 
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
Line -... Line 219...
- 
 
 219 
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
- 
 
 220 
 
- 
 
 221 
        //-----------------------------------------------------
217 
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
 222 
        // PPM state machine, onboard demultiplexed by HEF4017
218 
 
 223 
        //-----------------------------------------------------
219 
        if(PlatinenVersion < 20)
 - 
 
220 
        {
 - 
 
221 
                //---------------------------
 - 
 
222 
                // Nick servo state machine
 224 
        if(!PulseOutput) // pulse output complete
223 
                //---------------------------
 225 
        {
224 
                if(!PulseOutput) // pulse output complete
 226 
                if(TCCR2A & (1<<COM2A0)) // we had a low pulse
- 
 
 227 
                {
- 
 
 228 
                        TCCR2A &= ~(1<<COM2A0);// make a high pulse
- 
 
 229 
 
- 
 
 230 
                        if(ServoIndex == 0) // if we are at the sync gap
- 
 
 231 
                        {
- 
 
 232 
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
- 
 
 233 
                                ServoFrameTime = 0; // reset servo frame time
- 
 
 234 
                                HEF4017R_ON; // enable HEF4017 reset
225 
                {
 235 
                        }
226 
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
 - 
 
227 
                        {
 236 
                        else // servo channels
228 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
 - 
 
229 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 - 
 
230 
 
 - 
 
231 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 - 
 
232 
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
233 
                                if(EE_Parameter.ServoCompInvert & 0x01)
 - 
 
234 
                                {       // inverting movement of servo
 - 
 
235 
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
236 
                                }
 - 
 
237 
                                else
 - 
 
238 
                                {       // non inverting movement of servo
 - 
 
239 
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
240 
                                }
 - 
 
241 
                                // limit servo value to its parameter range definition
 - 
 
242 
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 - 
 
243 
                                {
 - 
 
244 
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 - 
 
245 
                                }
 - 
 
246 
                                else
 - 
 
247 
                                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 - 
 
248 
                                {
 - 
 
249 
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 - 
 
250 
                                }
 237 
                        {
251 
 
 - 
 
252 
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 - 
 
Line 253... Line 238...
253 
 
 238 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
254 
                                ServoNickValue /= MULTIPLYER;
 239 
                                RemainingPulse += 2 * servoValues[ServoIndex - 1]; // add channel value, factor of 2 because timer 1 increments 3.2µs
255 
 
 240 
 
256 
                                // range servo pulse width
 - 
 
257 
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
 - 
 
258 
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
 - 
 
259 
                                // accumulate time for correct update rate
 - 
 
260 
                                ServoFrameTime = RemainingPulse;
 - 
 
261 
                        }
 - 
 
262 
                        else // we had a high pulse
 - 
 
263 
                        {
 - 
 
264 
                                TCCR2A |= (1<<COM2A0); // make a low pulse
 - 
 
265 
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
 - 
 
266 
                        }
 - 
 
267 
                        // set pulse output active
 - 
 
268 
                        PulseOutput = 1;
 - 
 
269 
                }
 - 
 
270 
        } // EOF Nick servo state machine
 - 
 
271 
        else
 - 
 
272 
        {
 - 
 
273 
                //-----------------------------------------------------
 - 
 
274 
                // PPM state machine, onboard demultiplexed by HEF4017
 - 
 
275 
                //-----------------------------------------------------
 - 
 
276 
                if(!PulseOutput) // pulse output complete
 - 
 
277 
                {
 - 
 
278 
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
 - 
 
279 
                        {
 - 
 
280 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
 - 
 
281 
 
 - 
 
282 
                                if(ServoIndex == 0) // if we are at the sync gap
 - 
 
283 
                                {
 - 
 
284 
                                        RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
 - 
 
285 
                                        ServoFrameTime = 0; // reset servo frame time
 - 
 
286 
                                        HEF4017R_ON; // enable HEF4017 reset
 - 
 
287 
                                }
 - 
 
288 
                                else // servo channels
 - 
 
289 
                                {
 - 
 
290 
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 - 
 
291 
                                        switch(ServoIndex) // map servo channels
 - 
 
292 
                                        {
 - 
 
293 
                                                case 1: // Nick Compensation Servo
 - 
 
294 
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 - 
 
295 
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
296 
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
 - 
 
297 
                                                        {       // inverting movement of servo
 - 
 
298 
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
299 
                                                        }
 - 
 
300 
                                                        else
 - 
 
301 
                                                        {       // non inverting movement of servo
 - 
 
302 
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
303 
                                                        }
 - 
 
304 
                                                        // limit servo value to its parameter range definition
 - 
 
305 
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 - 
 
306 
                                                        {
 - 
 
307 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 - 
 
308 
                                                        }
 - 
 
309 
                                                        else
 - 
 
310 
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 - 
 
311 
                                                        {
 - 
 
312 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 - 
 
313 
                                                        }
 - 
 
314 
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 - 
 
315 
                                                        ServoNickValue /= MULTIPLYER;
 - 
 
316 
                                                        break;
 - 
 
317 
                                         case 2: // Roll Compensation Servo
 - 
 
318 
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
 - 
 
319 
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
320 
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
 - 
 
321 
                                                        {       // inverting movement of servo
 - 
 
322 
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
323 
                                                        }
 - 
 
324 
                                                        else
 - 
 
325 
                                                        {       // non inverting movement of servo
 - 
 
326 
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
327 
                                                        }
 - 
 
328 
                                                    // limit servo value to its parameter range definition
 - 
 
329 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
 - 
 
330 
                                                        {
 - 
 
331 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
 - 
 
332 
                                                        }
 - 
 
333 
                                                        else
 - 
 
334 
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
 - 
 
335 
                                                        {
 - 
 
336 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
 - 
 
337 
                                                        }
 - 
 
338 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 - 
 
339 
                                                        ServoRollValue /= MULTIPLYER;
 - 
 
340 
                                                        //DebugOut.Analog[20] = ServoRollValue;
 - 
 
341 
                                                        break;
 - 
 
342 
 
 - 
 
343 
                                                default: // other servo channels
 - 
 
344 
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
 - 
 
345 
                                                        break;
 - 
 
346 
                                        }
 - 
 
347 
                                        // range servo pulse width
 241 
                                // range servo pulse width
348 
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
 242 
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
349 
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
 - 
 
350 
                                        // substract stop pulse width
 - 
 
351 
                                        RemainingPulse -= PPM_STOPPULSE;
 - 
 
352 
                                        // accumulate time for correct sync gap
 - 
 
353 
                                        ServoFrameTime += RemainingPulse;
 - 
 
354 
                                }
 - 
 
355 
                        }
 - 
 
356 
                        else // we had a high pulse
 - 
 
357 
                        {
 - 
 
358 
                                TCCR2A |= (1<<COM2A0); // make a low pulse
 243 
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
359 
                                // set pulsewidth to stop pulse width
 244 
                                // substract stop pulse width
360 
                                RemainingPulse = PPM_STOPPULSE;
 - 
 
361 
                                // accumulate time for correct sync gap
 - 
 
362 
                                ServoFrameTime += RemainingPulse;
 - 
 
363 
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
 - 
 
364 
                                else HEF4017R_ON;
 245 
                                RemainingPulse -= PPM_STOPPULSE;
365 
                                ServoIndex++; // change to next servo channel
 - 
 
366 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
 - 
 
367 
                        }
 246 
                                // accumulate time for correct sync gap
- 
 
 247 
                                ServoFrameTime += RemainingPulse;
- 
 
 248 
                        }
- 
 
 249 
                }
- 
 
 250 
                else // we had a high pulse
- 
 
 251 
                {
- 
 
 252 
                        TCCR2A |= (1<<COM2A0); // make a low pulse
- 
 
 253 
                        // set pulsewidth to stop pulse width
- 
 
 254 
                        RemainingPulse = PPM_STOPPULSE;
- 
 
 255 
                        // accumulate time for correct sync gap
- 
 
 256 
                        ServoFrameTime += RemainingPulse;
- 
 
 257 
                        HEF4017R_OFF; // disable HEF4017 reset
368 
                        // set pulse output active
 258 
                        ServoIndex++; // change to next servo channel
- 
 
 259 
                        if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
- 
 
 260 
                }
Line 369... Line 261...
369 
                        PulseOutput = 1;
 261 
                // set pulse output active
370 
                }
 262 
                PulseOutput = 1;
371 
        } // EOF PPM state machine
 263 
        }
372 
 
 264