Subversion Repositories FlightCtrl

Rev

Rev 1400 | Show entire file | Regard 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 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 217... Line -...
217 
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
 - 
 
218 
 
 - 
 
219 
        if(PlatinenVersion < 20)
 - 
 
220 
        {
 - 
 
221 
                //---------------------------
 - 
 
222 
                // Nick servo state machine
 - 
 
223 
                //---------------------------
 - 
 
224 
                if(!PulseOutput) // pulse output complete
 - 
 
225 
                {
 - 
 
226 
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
 - 
 
227 
                        {
 - 
 
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 
                                }
 - 
 
251 
 
 - 
 
252 
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 - 
 
253 
 
 - 
 
254 
                                ServoNickValue /= MULTIPLYER;
 - 
 
255 
 
 - 
 
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
 219 
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
272 
        {
 220 
 
273 
                //-----------------------------------------------------
 221 
        //-----------------------------------------------------
274 
                // PPM state machine, onboard demultiplexed by HEF4017
 222 
        // PPM state machine, onboard demultiplexed by HEF4017
275 
                //-----------------------------------------------------
 223 
        //-----------------------------------------------------
Line 286... Line 234...
286 
                                        HEF4017R_ON; // enable HEF4017 reset
 234 
                                HEF4017R_ON; // enable HEF4017 reset
287 
                                }
 235 
                        }
288 
                                else // servo channels
 236 
                        else // servo channels
289 
                                {
 237 
                        {
290 
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 238 
                                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
 239 
                                RemainingPulse += 2 * servoValues[ServoIndex - 1]; // add channel value, factor of 2 because timer 1 increments 3.2µs
339 
                                                        ServoRollValue /= MULTIPLYER;
 - 
 
340 
                                                        //DebugOut.Analog[20] = ServoRollValue;
 - 
 
341 
                                                        break;
 - 
 
342 
 
 240 
 
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
 243 
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
350 
                                        // substract stop pulse width
 244 
                                // substract stop pulse width
351 
                                        RemainingPulse -= PPM_STOPPULSE;
 245 
                                RemainingPulse -= PPM_STOPPULSE;
Line 358... Line 252...
358 
                                TCCR2A |= (1<<COM2A0); // make a low pulse
 252 
                        TCCR2A |= (1<<COM2A0); // make a low pulse
359 
                                // set pulsewidth to stop pulse width
 253 
                        // set pulsewidth to stop pulse width
360 
                                RemainingPulse = PPM_STOPPULSE;
 254 
                        RemainingPulse = PPM_STOPPULSE;
361 
                                // accumulate time for correct sync gap
 255 
                        // accumulate time for correct sync gap
362 
                                ServoFrameTime += RemainingPulse;
 256 
                        ServoFrameTime += RemainingPulse;
363 
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
 257 
                        HEF4017R_OFF; // disable HEF4017 reset
364 
                                else HEF4017R_ON;
 - 
 
365 
                                ServoIndex++; // change to next servo channel
 258 
                        ServoIndex++; // change to next servo channel
366 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
 259 
                        if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
367 
                        }
 260 
                }
368 
                        // set pulse output active
 261 
                // set pulse output active
369 
                        PulseOutput = 1;
 262 
                PulseOutput = 1;
370 
                }
 263 
        }
371 
        } // EOF PPM state machine
 - 
 
Line 372... Line 264...
372 
 
 264 
 
373 
        // General pulse output generator
 265 
        // General pulse output generator
374 
        if(RemainingPulse > (255 + IRS_RUNTIME))
 266 
        if(RemainingPulse > (255 + IRS_RUNTIME))
375 
        {
 267 
        {