Rev 761 | Rev 764 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 761 | Rev 762 | ||
---|---|---|---|
Line 68... | Line 68... | ||
68 | #include "mm3.h" |
68 | #include "mm3.h" |
69 | #endif |
69 | #endif |
70 | #ifdef USE_CMPS03 |
70 | #ifdef USE_CMPS03 |
71 | #include "cmps03.h" |
71 | #include "cmps03.h" |
72 | #endif |
72 | #endif |
- | 73 | #include "led.h" |
|
Line 73... | Line 74... | ||
73 | 74 | ||
74 | volatile uint16_t I2CTimeout = 100; |
75 | volatile uint16_t I2CTimeout = 100; |
75 | // gyro readings |
76 | // gyro readings |
76 | volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
77 | volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
Line 112... | Line 113... | ||
112 | float Gyro_P_Factor; |
113 | float Gyro_P_Factor; |
113 | float Gyro_I_Factor; |
114 | float Gyro_I_Factor; |
Line 114... | Line 115... | ||
114 | 115 | ||
Line 115... | Line 116... | ||
115 | volatile int16_t DiffPitch, DiffRoll; |
116 | volatile int16_t DiffPitch, DiffRoll; |
Line 116... | Line 117... | ||
116 | 117 | ||
117 | int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
118 | int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
Line 118... | Line 119... | ||
118 | 119 | ||
Line 360... | Line 361... | ||
360 | //update poti values by rc-signals |
361 | //update poti values by rc-signals |
361 | if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
362 | if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
362 | if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
363 | if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
363 | if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
364 | if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
364 | if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
365 | if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--; |
- | 366 | ||
- | 367 | //PPM24-Extension |
|
- | 368 | if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--; |
|
- | 369 | if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--; |
|
- | 370 | if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--; |
|
- | 371 | if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--; |
|
- | 372 | ||
365 | //limit poti values |
373 | //limit poti values |
366 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
374 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
367 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
375 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
368 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
376 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
369 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
377 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
- | 378 | //PPM24-Extension |
|
- | 379 | if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255; |
|
- | 380 | if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
|
- | 381 | if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
|
- | 382 | if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
|
Line 370... | Line 383... | ||
370 | 383 | ||
371 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
384 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
372 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
385 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
Line 405... | Line 418... | ||
405 | /************************************************************************/ |
418 | /************************************************************************/ |
406 | /* Maps the parameter to poti values */ |
419 | /* Maps the parameter to poti values */ |
407 | /************************************************************************/ |
420 | /************************************************************************/ |
408 | void ParameterMapping(void) |
421 | void ParameterMapping(void) |
409 | { |
422 | { |
- | 423 | if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok |
|
- | 424 | // else the last updated values are used |
|
410 | 425 | { |
|
411 | #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
426 | #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
412 | CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255); |
427 | CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255); |
413 | CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100); |
428 | CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100); |
414 | CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100); |
429 | CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100); |
415 | CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255); |
430 | CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255); |
Line 428... | Line 443... | ||
428 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
443 | CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
429 | CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255); |
444 | CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255); |
430 | CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
445 | CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
431 | CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
446 | CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
432 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
447 | CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
433 | - | ||
434 | Ki = (float) FCParam.I_Factor * FACTOR_I; |
448 | Ki = (float) FCParam.I_Factor * FACTOR_I; |
435 | } |
449 | } |
- | 450 | } |
|
Line 436... | Line 451... | ||
436 | 451 | ||
437 | 452 | ||
438 | /************************************************************************/ |
453 | /************************************************************************/ |
Line 454... | Line 469... | ||
454 | static int8_t TimerDebugOut = 0; |
469 | static int8_t TimerDebugOut = 0; |
455 | static int8_t StoreNewCompassCourse = 0; |
470 | static int8_t StoreNewCompassCourse = 0; |
456 | static int32_t CorrectionPitch, CorrectionRoll; |
471 | static int32_t CorrectionPitch, CorrectionRoll; |
Line 457... | Line 472... | ||
457 | 472 | ||
458 | Mean(); |
- | |
459 | 473 | Mean(); |
|
- | 474 | GRN_ON; |
|
460 | GRN_ON; |
475 | |
461 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
476 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
462 | // determine thrust value |
477 | // determine thrust value |
463 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
478 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
464 | ThrustMixFraction = StickThrust; |
479 | ThrustMixFraction = StickThrust; |
Line 498... | Line 513... | ||
498 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
513 | PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
499 | PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
514 | PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
500 | PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
515 | PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
501 | } |
516 | } |
502 | else MotorsOn = 0; // switch of all motors |
517 | else MotorsOn = 0; // switch of all motors |
503 | } |
518 | } // eof SenderOkay < 100 |
504 | else |
519 | else |
505 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
520 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
506 | // RC-signal is good |
521 | // RC-signal is good |
507 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
522 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
508 | if(SenderOkay > 140) |
523 | if(SenderOkay > 140) |
Line 622... | Line 637... | ||
622 | if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
637 | if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
623 | { |
638 | { |
624 | delay_stopmotors = 200; // do not repeat if once executed |
639 | delay_stopmotors = 200; // do not repeat if once executed |
625 | Modell_Is_Flying = 0; |
640 | Modell_Is_Flying = 0; |
626 | MotorsOn = 0; |
641 | MotorsOn = 0; |
627 | - | ||
- | 642 | GPS_ClearHomePosition(); |
|
628 | } |
643 | } |
629 | } |
644 | } |
630 | else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
645 | else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
631 | } |
646 | } |
- | 647 | // remapping of paameters only if the signal rc-sigbnal conditions are good |
|
632 | } |
648 | } // eof SenderOkay > 140 |
633 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
649 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
634 | // new values from RC |
650 | // new values from RC |
635 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
651 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
636 | if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC |
652 | if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC |
637 | { |
653 | { |
638 | int tmp_int; |
654 | int tmp_int; |
639 | ParameterMapping(); // remapping params (online poti replacement) |
655 | ParameterMapping(); // remapping params (online poti replacement) |
640 | - | ||
641 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
656 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
642 | StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
657 | StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
643 | StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
658 | StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
644 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
659 | StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
645 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
660 | StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
Line 752... | Line 767... | ||
752 | if(Looping_Roll || Looping_Pitch) |
767 | if(Looping_Roll || Looping_Pitch) |
753 | { |
768 | { |
754 | if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit; |
769 | if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit; |
755 | } |
770 | } |
Line -... | Line 771... | ||
- | 771 | ||
- | 772 | ||
- | 773 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 774 | //+ LED Control on J16/J17 |
|
- | 775 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 776 | LED_OffTime = FCParam.UserParam7; |
|
- | 777 | LED_OnTime = FCParam.UserParam8; |
|
- | 778 | LED_Update(); |
|
756 | 779 | ||
757 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
780 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
758 | // in case of emergency landing |
781 | // in case of emergency landing |
759 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
782 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
760 | // set all inputs to save values |
783 | // set all inputs to save values |
Line 1054... | Line 1077... | ||
1054 | } |
1077 | } |
1055 | } |
1078 | } |
1056 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1079 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1057 | // GPS |
1080 | // GPS |
1058 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1081 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1059 | if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding) |
1082 | if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
1060 | { |
1083 | { |
1061 | GPS_P_Factor = FCParam.UserParam5; |
1084 | GPS_P_Factor = FCParam.UserParam5; |
1062 | GPS_D_Factor = FCParam.UserParam6; |
1085 | GPS_D_Factor = FCParam.UserParam6; |
1063 | GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data |
1086 | if(EmergencyLanding) GPS_Main(230); // enables Comming Home |
- | 1087 | else GPS_Main(Poti3); |
|
1064 | } |
1088 | } |
1065 | else |
1089 | else |
1066 | { |
1090 | { |
1067 | GPS_Neutral(); |
1091 | GPS_Neutral(); |
1068 | } |
1092 | } |