Subversion Repositories FlightCtrl

Rev

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

Rev 1713 Rev 1760
Line 1... Line 1...
1 
#include "main.h"
 1 
#include "main.h"
2 
 
 - 
 
- 
 
 2 
#define MULTIPLYER 4
Line 3... Line 3...
3 
 
 3 
 
4 
volatile unsigned int CountMilliseconds = 0;
 4 
volatile unsigned int CountMilliseconds = 0;
5 
volatile static unsigned int tim_main;
 5 
volatile static unsigned int tim_main;
6 
volatile unsigned char UpdateMotor = 0;
 6 
volatile unsigned char UpdateMotor = 0;
7 
volatile unsigned int cntKompass = 0;
 7 
volatile unsigned int cntKompass = 0;
8 
volatile unsigned int beeptime = 0;
 8 
volatile unsigned int beeptime = 0;
- 
 
 9 
volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1;
- 
 
 10 
uint16_t RemainingPulse = 0;
- 
 
 11 
volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
Line 9... Line 12...
9 
volatile unsigned char SendSPI = 0, ServoActive = 0;
 12 
volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon
Line 10... Line 13...
10 
 
 13 
 
11 
unsigned int BeepMuster = 0xffff;
 14 
unsigned int BeepMuster = 0xffff;
Line 183... Line 186...
183 
 
 186 
 
184 
/*****************************************************/
 187 
/*****************************************************/
185 
/*              Control Servo Position               */
 188 
/*              Control Servo Position               */
Line -... Line 189...
- 
 
 189 
/*****************************************************/
- 
 
 190 
 
- 
 
 191 
 
- 
 
 192 
void CalculateServo(void)
- 
 
 193 
{
- 
 
 194 
 long cosinus, sinus;
- 
 
 195 
 long nick, roll;
- 
 
 196 
 
- 
 
 197 
        cosinus = sintab[EE_Parameter.CamOrientation + 6];
- 
 
 198 
        sinus = sintab[EE_Parameter.CamOrientation];
- 
 
 199 
 
- 
 
 200 
  if(CalculateServoSignals == 1)
- 
 
 201 
   {
- 
 
 202 
    nick = ((cosinus * (IntegralNick / (8 * 128L))) - (sinus * (IntegralRoll / (8 * 128L))));
- 
 
 203 
//nick = (IntegralNick / 128L);
- 
 
 204 
 
- 
 
 205 
                                                        ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed;
- 
 
 206 
                                                        ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765)
- 
 
 207 
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
- 
 
 208 
                                                        {       // inverting movement of servo
- 
 
 209 
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) );
- 
 
 210 
                                                        }
- 
 
 211 
                                                        else
- 
 
 212 
                                                        {       // non inverting movement of servo
- 
 
 213 
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) );
- 
 
 214 
                                                        }
- 
 
 215 
                                                        // limit servo value to its parameter range definition
- 
 
 216 
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
- 
 
 217 
                                                        {
- 
 
 218 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
- 
 
 219 
                                                        }
- 
 
 220 
                                                        else
- 
 
 221 
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
- 
 
 222 
                                                        {
- 
 
 223 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
- 
 
 224 
                                                        }
- 
 
 225 
                                                        if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
- 
 
 226 
        }
- 
 
 227 
        else
- 
 
 228 
        {
- 
 
 229 
    roll = ((cosinus * (IntegralRoll / (8 * 128L))) + (sinus * (IntegralNick / (8 * 128L))));
- 
 
 230 
 
- 
 
 231 
                                                        ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
- 
 
 232 
                                                        ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765)
- 
 
 233 
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
- 
 
 234 
                                                        {       // inverting movement of servo
- 
 
 235 
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) );
- 
 
 236 
                                                        }
- 
 
 237 
                                                        else
- 
 
 238 
                                                        {       // non inverting movement of servo
- 
 
 239 
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) );
- 
 
 240 
                                                        }
- 
 
 241 
                                                    // limit servo value to its parameter range definition
- 
 
 242 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
- 
 
 243 
                                                        {
- 
 
 244 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
- 
 
 245 
                                                        }
- 
 
 246 
                                                        else
- 
 
 247 
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
- 
 
 248 
                                                        {
- 
 
 249 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
- 
 
 250 
                                                        }
- 
 
 251 
                                                        CalculateServoSignals = 0;
- 
 
 252 
        }
186 
/*****************************************************/
 253 
}
187 
 
 254 
 
188 
ISR(TIMER2_COMPA_vect)
 255 
ISR(TIMER2_COMPA_vect)
189 
{
 256 
{
190 
        // frame len 22.5 ms = 14063 * 1.6 us
 257 
        // frame len 22.5 ms = 14063 * 1.6 us
191 
        // stop pulse: 0.3 ms = 188 * 1.6 us
 258 
        // stop pulse: 0.3 ms = 188 * 1.6 us
192 
        // min servo pulse: 0.6 ms =  375 * 1.6 us
 259 
        // min servo pulse: 0.6 ms =  375 * 1.6 us
Line 193... Line 260...
193 
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
 260 
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
194 
        // resolution: 1500 - 375 = 1125 steps
 261 
        // resolution: 1500 - 375 = 1125 steps
195 
 
 - 
 
196 
        #define IRS_RUNTIME 127
 262 
 
197 
        #define PPM_STOPPULSE 188
 263 
        #define IRS_RUNTIME 127
198 
//      #define PPM_FRAMELEN (14063
 264 
        #define PPM_STOPPULSE 188
199 
    #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
 265 
    #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
Line 200... Line 266...
200 
        #define MINSERVOPULSE 375
 266 
        #define MINSERVOPULSE 375
201 
        #define MAXSERVOPULSE 1500
 - 
 
202 
        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
 267 
        #define MAXSERVOPULSE 1500
203 
 
 268 
        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
Line 204... Line -...
204 
        static uint8_t  PulseOutput = 0;
 - 
 
205 
        static uint16_t RemainingPulse = 0;
 - 
 
206 
        static uint16_t ServoFrameTime = 0;
 - 
 
Line 207... Line 269...
207 
        static uint8_t  ServoIndex = 0;
 269 
 
208 
 
 270 
        static uint8_t  PulseOutput = 0;
209 
        #define MULTIPLYER 4
 271 
        static uint16_t ServoFrameTime = 0;
210 
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
 272 
        static uint8_t  ServoIndex = 0;
Line 219... Line 281...
219 
                {
 281 
                {
220 
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
 282 
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
221 
                        {
 283 
                        {
222 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
 284 
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
223 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 285 
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
224 
 
 - 
 
225 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 - 
 
226 
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
227 
                                if(EE_Parameter.ServoCompInvert & 0x01)
 - 
 
228 
                                {       // inverting movement of servo
 - 
 
229 
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
230 
                                }
 - 
 
231 
                                else
 - 
 
232 
                                {       // non inverting movement of servo
 - 
 
233 
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
234 
                                }
 - 
 
235 
                                // limit servo value to its parameter range definition
 - 
 
236 
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 - 
 
237 
                                {
 - 
 
238 
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 - 
 
239 
                                }
 - 
 
240 
                                else
 - 
 
241 
                                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 - 
 
242 
                                {
 - 
 
243 
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 - 
 
244 
                                }
 - 
 
245 
 
 - 
 
246 
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 286 
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
247 
 
 - 
 
248 
                                ServoNickValue /= MULTIPLYER;
 - 
 
249 
 
 - 
 
250 
                                // range servo pulse width
 287 
                                // range servo pulse width
251 
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
 288 
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
252 
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
 289 
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
253 
                                // accumulate time for correct update rate
 290 
                                // accumulate time for correct update rate
254 
                                ServoFrameTime = RemainingPulse;
 291 
                                ServoFrameTime = RemainingPulse;
Line 283... Line 320...
283 
                                {
 320 
                                {
284 
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
 321 
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
285 
                                        switch(ServoIndex) // map servo channels
 322 
                                        switch(ServoIndex) // map servo channels
286 
                                        {
 323 
                                        {
287 
                                                case 1: // Nick Compensation Servo
 324 
                                                case 1: // Nick Compensation Servo
288 
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
 - 
 
289 
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
290 
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
 - 
 
291 
                                                        {       // inverting movement of servo
 - 
 
292 
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
293 
                                                        }
 - 
 
294 
                                                        else
 - 
 
295 
                                                        {       // non inverting movement of servo
 - 
 
296 
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
 - 
 
297 
                                                        }
 - 
 
298 
                                                        // limit servo value to its parameter range definition
 - 
 
299 
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 - 
 
300 
                                                        {
 - 
 
301 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 - 
 
302 
                                                        }
 - 
 
303 
                                                        else
 - 
 
304 
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 - 
 
305 
                                                        {
 - 
 
306 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 - 
 
307 
                                                        }
 - 
 
308 
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 325 
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
309 
                                                        ServoNickValue /= MULTIPLYER;
 - 
 
310 
                                                        break;
 326 
                                                        break;
311 
                                         case 2: // Roll Compensation Servo
 327 
                                         case 2: // Roll Compensation Servo
312 
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
 - 
 
313 
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
 - 
 
314 
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
 - 
 
315 
                                                        {       // inverting movement of servo
 - 
 
316 
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
317 
                                                        }
 - 
 
318 
                                                        else
 - 
 
319 
                                                        {       // non inverting movement of servo
 - 
 
320 
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
 - 
 
321 
                                                        }
 - 
 
322 
                                                    // limit servo value to its parameter range definition
 - 
 
323 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
 - 
 
324 
                                                        {
 - 
 
325 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
 - 
 
326 
                                                        }
 - 
 
327 
                                                        else
 - 
 
328 
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
 - 
 
329 
                                                        {
 - 
 
330 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
 - 
 
331 
                                                        }
 - 
 
332 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
 328 
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
333 
                                                        ServoRollValue /= MULTIPLYER;
 - 
 
334 
                                                        //DebugOut.Analog[20] = ServoRollValue;
 - 
 
335 
                                                        break;
 329 
                                                        break;
336 
                                         case 3:
 330 
                                         case 3:
337 
                                                        RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
 331 
                                                        RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
338 
                                                        break;
 332 
                                                        break;
339 
                                         case 4:
 333 
                                         case 4:
Line 363... Line 357...
363 
                                // accumulate time for correct sync gap
 357 
                                // accumulate time for correct sync gap
364 
                                ServoFrameTime += RemainingPulse;
 358 
                                ServoFrameTime += RemainingPulse;
365 
                                if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
 359 
                                if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
366 
                                else HEF4017R_ON;
 360 
                                else HEF4017R_ON;
367 
                                ServoIndex++; // change to next servo channel
 361 
                                ServoIndex++; // change to next servo channel
368 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
 362 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh)
- 
 
 363 
                                  {
- 
 
 364 
                                    CalculateServoSignals = 1;
- 
 
 365 
                                        ServoIndex = 0; // reset to the sync gap
- 
 
 366 
                                  }
369 
                        }
 367 
                        }
370 
                        // set pulse output active
 368 
                        // set pulse output active
371 
                        PulseOutput = 1;
 369 
                        PulseOutput = 1;
372 
                }
 370 
                }
373 
        } // EOF PPM state machine
 371 
        } // EOF PPM state machine