WebSVN - FlightCtrl - Diff - Rev 762 and 764 - /branches/V0.68d Code Redesign killagreg/fc.c



        static int32_t SetPointYaw = 0;
        static int32_t IntegralErrorPitch = 0;
        static int32_t IntegralErrorRoll = 0;
        static uint16_t RcLostTimer;
        static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
        static uint16_t Model_Is_Flying = 0;
        static uint8_t HeightControlActive = 0;
        static int16_t HeightControlThrust = 0;
        static int8_t TimerDebugOut = 0;
        static int8_t StoreNewCompassCourse = 0;
        static int32_t CorrectionPitch, CorrectionRoll;

                {
                  MotorsOn = 0; // stop all motors
                  EmergencyLanding = 0; // emergency landing is over
                }
                ROT_ON; // set red led
                if(Model_Is_Flying > 2000)  // wahrscheinlich in der Luft --> langsam absenken
                {
                        ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust
                        EmergencyLanding = 1; // enable emergency landing
                        // set neutral rc inputs
                        PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0;

                EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
                // reset emergency timer
                RcLostTimer = ParamSet.EmergencyThrustDuration * 50;
                if(ThrustMixFraction > 40)
                {
                        if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
                }
                if((Model_Is_Flying < 200) || (ThrustMixFraction < 40))
                {
                        SumPitch = 0;
                        SumRoll = 0;
                        Reading_IntegralGyroYaw = 0;
                        Reading_IntegralGyroYaw2 = 0;

                        {
                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_neutral = 0;
                                        GRN_OFF;
                                        Model_Is_Flying = 0;
                                        // check roll/pitch stick position
                                        // if pitch stick is topmost or roll stick is leftmost --> change parameter setting
                                        // according to roll/pitch stick position
                                        if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
                                        {

                        if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                delay_neutral = 0;
                                GRN_OFF;
                                SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid
                                Model_Is_Flying = 0;
                                SetNeutral();
                                // Save ACC neutral settings to eeprom
                                SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
                                SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
                                SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);

                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
                        {
                                if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_startmotors = 200; // do not repeat if once executed
                                        Model_Is_Flying = 1;
                                        MotorsOn = 1;
                                        SetPointYaw = 0;
                                        Reading_IntegralGyroYaw = 0;
                                        Reading_IntegralGyroYaw2 = 0;
                                        Reading_IntegralGyroPitch = 0;

                        if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
                                {
                                if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
                                {
                                        delay_stopmotors = 200; // do not repeat if once executed
                                        Model_Is_Flying = 0;
                                        MotorsOn = 0;
                                        GPS_ClearHomePosition();
                                }
                        }
                        else delay_stopmotors = 0; // reset delay timer if sticks are not in this position

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

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c - Rev 762 → 764