Rev 762 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 762 | Rev 764 | ||
---|---|---|---|
Line 461... | Line 461... | ||
461 | static int32_t SetPointYaw = 0; |
461 | static int32_t SetPointYaw = 0; |
462 | static int32_t IntegralErrorPitch = 0; |
462 | static int32_t IntegralErrorPitch = 0; |
463 | static int32_t IntegralErrorRoll = 0; |
463 | static int32_t IntegralErrorRoll = 0; |
464 | static uint16_t RcLostTimer; |
464 | static uint16_t RcLostTimer; |
465 | static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
465 | static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
466 | static uint16_t Modell_Is_Flying = 0; |
466 | static uint16_t Model_Is_Flying = 0; |
467 | static uint8_t HeightControlActive = 0; |
467 | static uint8_t HeightControlActive = 0; |
468 | static int16_t HeightControlThrust = 0; |
468 | static int16_t HeightControlThrust = 0; |
469 | static int8_t TimerDebugOut = 0; |
469 | static int8_t TimerDebugOut = 0; |
470 | static int8_t StoreNewCompassCourse = 0; |
470 | static int8_t StoreNewCompassCourse = 0; |
471 | static int32_t CorrectionPitch, CorrectionRoll; |
471 | static int32_t CorrectionPitch, CorrectionRoll; |
Line 501... | Line 501... | ||
501 | { |
501 | { |
502 | MotorsOn = 0; // stop all motors |
502 | MotorsOn = 0; // stop all motors |
503 | EmergencyLanding = 0; // emergency landing is over |
503 | EmergencyLanding = 0; // emergency landing is over |
504 | } |
504 | } |
505 | ROT_ON; // set red led |
505 | ROT_ON; // set red led |
506 | if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
506 | if(Model_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
507 | { |
507 | { |
508 | ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust |
508 | ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust |
509 | EmergencyLanding = 1; // enable emergency landing |
509 | EmergencyLanding = 1; // enable emergency landing |
510 | // set neutral rc inputs |
510 | // set neutral rc inputs |
511 | PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
511 | PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
Line 525... | Line 525... | ||
525 | EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
525 | EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
526 | // reset emergency timer |
526 | // reset emergency timer |
527 | RcLostTimer = ParamSet.EmergencyThrustDuration * 50; |
527 | RcLostTimer = ParamSet.EmergencyThrustDuration * 50; |
528 | if(ThrustMixFraction > 40) |
528 | if(ThrustMixFraction > 40) |
529 | { |
529 | { |
530 | if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
530 | if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++; |
531 | } |
531 | } |
532 | if((Modell_Is_Flying < 200) || (ThrustMixFraction < 40)) |
532 | if((Model_Is_Flying < 200) || (ThrustMixFraction < 40)) |
533 | { |
533 | { |
534 | SumPitch = 0; |
534 | SumPitch = 0; |
535 | SumRoll = 0; |
535 | SumRoll = 0; |
536 | Reading_IntegralGyroYaw = 0; |
536 | Reading_IntegralGyroYaw = 0; |
537 | Reading_IntegralGyroYaw2 = 0; |
537 | Reading_IntegralGyroYaw2 = 0; |
Line 547... | Line 547... | ||
547 | { |
547 | { |
548 | if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
548 | if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
549 | { |
549 | { |
550 | delay_neutral = 0; |
550 | delay_neutral = 0; |
551 | GRN_OFF; |
551 | GRN_OFF; |
552 | Modell_Is_Flying = 0; |
552 | Model_Is_Flying = 0; |
553 | // check roll/pitch stick position |
553 | // check roll/pitch stick position |
554 | // if pitch stick is topmost or roll stick is leftmost --> change parameter setting |
554 | // if pitch stick is topmost or roll stick is leftmost --> change parameter setting |
555 | // according to roll/pitch stick position |
555 | // according to roll/pitch stick position |
556 | if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
556 | if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
557 | { |
557 | { |
Line 588... | Line 588... | ||
588 | if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
588 | if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
589 | { |
589 | { |
590 | delay_neutral = 0; |
590 | delay_neutral = 0; |
591 | GRN_OFF; |
591 | GRN_OFF; |
592 | SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
592 | SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
593 | Modell_Is_Flying = 0; |
593 | Model_Is_Flying = 0; |
594 | SetNeutral(); |
594 | SetNeutral(); |
595 | // Save ACC neutral settings to eeprom |
595 | // Save ACC neutral settings to eeprom |
596 | SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
596 | SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
597 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
597 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
598 | SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
598 | SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
Line 612... | Line 612... | ||
612 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
612 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
613 | { |
613 | { |
614 | if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
614 | if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
615 | { |
615 | { |
616 | delay_startmotors = 200; // do not repeat if once executed |
616 | delay_startmotors = 200; // do not repeat if once executed |
617 | Modell_Is_Flying = 1; |
617 | Model_Is_Flying = 1; |
618 | MotorsOn = 1; |
618 | MotorsOn = 1; |
619 | SetPointYaw = 0; |
619 | SetPointYaw = 0; |
620 | Reading_IntegralGyroYaw = 0; |
620 | Reading_IntegralGyroYaw = 0; |
621 | Reading_IntegralGyroYaw2 = 0; |
621 | Reading_IntegralGyroYaw2 = 0; |
622 | Reading_IntegralGyroPitch = 0; |
622 | Reading_IntegralGyroPitch = 0; |
Line 635... | Line 635... | ||
635 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
635 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
636 | { |
636 | { |
637 | 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) |
638 | { |
638 | { |
639 | delay_stopmotors = 200; // do not repeat if once executed |
639 | delay_stopmotors = 200; // do not repeat if once executed |
640 | Modell_Is_Flying = 0; |
640 | Model_Is_Flying = 0; |
641 | MotorsOn = 0; |
641 | MotorsOn = 0; |
642 | GPS_ClearHomePosition(); |
642 | GPS_ClearHomePosition(); |
643 | } |
643 | } |
644 | } |
644 | } |
645 | 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 |