WebSVN - FlightCtrl - Diff - Rev 1760 and 1765



const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};

 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
int MaxStickNick = 0,MaxStickRoll = 0;
unsigned int  modell_fliegt = 0;
volatile unsigned char FC_StatusFlags = 0;
long GIER_GRAD_FAKTOR = 1291;
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
signed int tmp_motorwert[MAX_MOTORS];

    ExternHoehenValue = 0;
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
    GierGyroFehler = 0;
    SendVersionToNavi = 1;
    LED_Init();
    FC_StatusFlags |= FC_STATUS_CALIBRATE;
    FromNaviCtrl_Value.Kalman_K = -1;
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

 

         {
                HEF4017R_ON;
                DDRD  |=0x80; // enable J7 -> Servo signal
     }

 
        if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };
        if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; };
        if((AdNeutralGier < 150 * 2)  || (AdNeutralGier > 850 * 2))  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; };
        if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; };
        if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; };
        if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };

}
 

//############################################################################
{
 unsigned char i;
    if(!MotorenEin)
        {
         FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN | FC_STATUS_FLY);
                 for(i=0;i<MAX_MOTORS;i++)
                  {
                   if(!PC_MotortestActive)  MotorTest[i] = 0;
                   Motor[i].SetPoint = MotorTest[i];
                   Motor[i].SetPointLowerBits = 0;

 Motor[i].SetPointLowerBits = MotorTest[i] % 4;
*/
                  }
          if(PC_MotortestActive) PC_MotortestActive--;
        }
        else FC_StatusFlags |= FC_STATUS_MOTOR_RUN;

 
    if(I2C_TransferActive)
         {
          I2C_TransferActive = 0; // enable for the next time
         }
        else
    {
     motor_write = 0;
     I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
        }

}

#ifdef SWITCH_LEARNS_CAREFREE
    if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
#endif
        CareFree = 1;
    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
    if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE;
   }
   else CareFree = 0;

 
 if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert
        {
         beeptime = 15000;
         BeepMuster = 0xA400;
         CareFree = 0;

    }
 

 if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}

        {
        if(RcLostTimer) RcLostTimer--;
        else
         {
          MotorenEin = 0;
          FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING;
         }
        ROT_ON;
        if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
            {
            GasMischanteil = EE_Parameter.NotGas;
            FC_StatusFlags |= FC_STATUS_EMERGENCY_LANDING;
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Emfang gut
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(SenderOkay > 140)
            {
                        FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING;
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
            if(GasMischanteil > 40 && MotorenEin)
                {
                if(modell_fliegt < 0xffff) modell_fliegt++;
                }

                Mess_Integral_Gier = 0;
                if(modell_fliegt == 250)
                 {
                  NeueKompassRichtungMerken = 1;
                 }
                } else FC_StatusFlags |= FC_STATUS_FLY;

 
            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
                {
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                            {
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                            }
                                                   ServoActive = 0;
                           SetNeutral(0);
                           calibration_done = 1;
                                                   ServoActive = 1;
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
                           Piep(GetActiveParamSet(),120);
                         }
                        }

                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
                                                                        Mess_IntegralNick2 = IntegralNick;
                                                                        Mess_IntegralRoll2 = IntegralRoll;
                                                                        SummeNick = 0;
                                                                        SummeRoll = 0;
                                                                        FC_StatusFlags |= FC_STATUS_START;
                                                                        ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
                                                                }
                                                                else
                                                                {
                                                                        beeptime = 1500; // indicate missing calibration

            }
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// neue Werte von der Funke
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 
 if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
  {
        static int stick_nick,stick_roll;
    ParameterZuordnung();
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;

     {
      MaxStickRoll = abs(StickRoll)/STICK_GAIN;
      if(MaxStickRoll > 100) MaxStickRoll = 100;
     }
     else MaxStickRoll--;
    if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)  {MaxStickNick = 0; MaxStickRoll = 0;}

 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)
    {
     StickGier = 0;
     StickNick = 0;

                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
                CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
                VarioCharacter = ' ';
                if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
                {
                        #define HEIGHT_TRIM_UP          0x01
                        #define HEIGHT_TRIM_DOWN        0x02
                        static unsigned char HeightTrimmingFlag = 0x00;

                  {
                // alternative height control
                // PD-Control with respect to hoover point
                // the thrust loss out of horizontal attitude is compensated
                // the setpoint will be fine adjusted with the gas stick position
                        if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying
                        {   // gas stick is above hoover point
                                if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
                                {
                                        if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
                                        {

                                                SollHoehe = HoehenWert; // update setpoint to current height
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
                                                if(!StartTrigger && HoehenWert > 50)
                                                {
                                                 StartTrigger = 1;
                                                }
                                        }
                                        VarioCharacter = '=';
                                }
                                // Trim height set point
                                if(abs(HeightTrimming) > 512)

                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
                                GasReduction = tmp_long;
                                // ------------------------- D-Part 1: Vario Meter ----------------------------
                                tmp_int = VarioMeter / 8;
                                LIMIT_MIN_MAX(tmp_int, -127, 128);
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
                                if(HeightTrimmingFlag)  tmp_int /= 4; // reduce d-part while trimming setpoint
                                else
                                if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
                                GasReduction += tmp_int;
                        } // EOF no baro range expanding
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
            if(Parameter_Hoehe_ACC_Wirkung)
                         {
                          tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
                          LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
                          GasReduction += tmp_long;
                         }
                        // ------------------------ D-Part 3: GpsZ  ----------------------------------
                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
            LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
                        GasReduction += tmp_int;
            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
                        // ------------------------                  ----------------------------------
                        HCGas -= GasReduction;
                        // limit deviation from hoover point within the target region
                        if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
                        {
                         unsigned int tmp;
                         tmp = abs(HeightDeviation);
                         if(tmp <= 60)
                         {
                          LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point
                         }
                         else
                         {
                                tmp = (tmp - 60) / 32;
                                if(tmp > 15) tmp = 15;
                           if(HeightDeviation > 0)
                                {
                                tmp = (HoverGasMin * (16 - tmp)) / 16;

                        // set GasMischanteil to HeightControlGasFilter
            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
                        {  // old version
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
                                GasMischanteil = FilterHCGas;
                        }
                        else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
                  }
                }// EOF height control active
                else // HC not active
                {

                        FilterHCGas = GasMischanteil;
                }

 
                // Hover gas estimation by averaging gas control output on small z-velocities
                // this is done only if height contol option is selected in global config and aircraft is flying
                if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING))
                {
                        if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
                        if(StartTrigger == 1) StartTrigger = 2;
                                tmp_long2 = (int32_t)GasMischanteil; // take current thrust

                                else
                                {       // no limit
                                        HoverGasMin = 0;
                                        HoverGasMax = 1023;
                                }
                }
                 else
                  {
                   StartTrigger = 0;
                   HoverGasFilter = 0;
                   HoverGas = 0;
                  }
        }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
        // limit gas to parameter setting
  LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;

Rev 1760	Rev 1765
Line 152...	Line 152...
152	const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};	152	const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};
Line 153...	Line 153...
153		153
154	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;	154	signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
155	int MaxStickNick = 0,MaxStickRoll = 0;	155	int MaxStickNick = 0,MaxStickRoll = 0;
156	unsigned int modell_fliegt = 0;	156	unsigned int modell_fliegt = 0;
157	volatile unsigned char FCFlags = 0;	157	volatile unsigned char FC_StatusFlags = 0;
158	long GIER_GRAD_FAKTOR = 1291;	158	long GIER_GRAD_FAKTOR = 1291;
159	signed int KopplungsteilNickRoll,KopplungsteilRollNick;	159	signed int KopplungsteilNickRoll,KopplungsteilRollNick;
160	signed int tmp_motorwert[MAX_MOTORS];	160	signed int tmp_motorwert[MAX_MOTORS];
Line 334...	Line 334...
334	ExternHoehenValue = 0;	334	ExternHoehenValue = 0;
335	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;	335	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
336	GierGyroFehler = 0;	336	GierGyroFehler = 0;
337	SendVersionToNavi = 1;	337	SendVersionToNavi = 1;
338	LED_Init();	338	LED_Init();
339	FCFlags \|= FCFLAG_CALIBRATE;	339	FC_StatusFlags \|= FC_STATUS_CALIBRATE;
340	FromNaviCtrl_Value.Kalman_K = -1;	340	FromNaviCtrl_Value.Kalman_K = -1;
341	FromNaviCtrl_Value.Kalman_MaxDrift = 0;	341	FromNaviCtrl_Value.Kalman_MaxDrift = 0;
342	FromNaviCtrl_Value.Kalman_MaxFusion = 32;	342	FromNaviCtrl_Value.Kalman_MaxFusion = 32;
Line 343...	Line 343...
343		343
Line 350...	Line 350...
350	{	350	{
351	HEF4017R_ON;	351	HEF4017R_ON;
352	DDRD \|=0x80; // enable J7 -> Servo signal	352	DDRD \|=0x80; // enable J7 -> Servo signal
353	}	353	}
Line 354...	Line 354...
354		354
355	if((AdNeutralNick < 150 * 16) \|\| (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] \|= DEFEKT_G_NICK; };	355	if((AdNeutralNick < 150 * 16) \|\| (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_GYRO_NICK; };
356	if((AdNeutralRoll < 150 * 16) \|\| (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] \|= DEFEKT_G_ROLL; };	356	if((AdNeutralRoll < 150 * 16) \|\| (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_GYRO_ROLL; };
357	if((AdNeutralGier < 150 * 2) \|\| (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] \|= DEFEKT_G_GIER; };	357	if((AdNeutralGier < 150 * 2) \|\| (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_GYRO_YAW; };
358	if((NeutralAccX < 300) \|\| (NeutralAccX > 750)) { VersionInfo.HardwareError[0] \|= DEFEKT_A_NICK; };	358	if((NeutralAccX < 300) \|\| (NeutralAccX > 750)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_ACC_NICK; };
359	if((NeutralAccY < 300) \|\| (NeutralAccY > 750)) { VersionInfo.HardwareError[0] \|= DEFEKT_A_ROLL; };	359	if((NeutralAccY < 300) \|\| (NeutralAccY > 750)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_ACC_ROLL; };
360	if((NeutralAccZ < 512) \|\| (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] \|= DEFEKT_A_Z; };	360	if((NeutralAccZ < 512) \|\| (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] \|= FC_ERROR0_ACC_TOP; };
Line 361...	Line 361...
361	}	361	}
362		362
Line 526...	Line 526...
526	//############################################################################	526	//############################################################################
527	{	527	{
528	unsigned char i;	528	unsigned char i;
529	if(!MotorenEin)	529	if(!MotorenEin)
530	{	530	{
531	FCFlags &= ~(FCFLAG_MOTOR_RUN \| FCFLAG_FLY);	531	FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN \| FC_STATUS_FLY);
532	for(i=0;i<MAX_MOTORS;i++)	532	for(i=0;i<MAX_MOTORS;i++)
533	{	533	{
534	if(!PC_MotortestActive) MotorTest[i] = 0;	534	if(!PC_MotortestActive) MotorTest[i] = 0;
535	Motor[i].SetPoint = MotorTest[i];	535	Motor[i].SetPoint = MotorTest[i];
536	Motor[i].SetPointLowerBits = 0;	536	Motor[i].SetPointLowerBits = 0;
Line 539...	Line 539...
539	Motor[i].SetPointLowerBits = MotorTest[i] % 4;	539	Motor[i].SetPointLowerBits = MotorTest[i] % 4;
540	*/	540	*/
541	}	541	}
542	if(PC_MotortestActive) PC_MotortestActive--;	542	if(PC_MotortestActive) PC_MotortestActive--;
543	}	543	}
544	else FCFlags \|= FCFLAG_MOTOR_RUN;	544	else FC_StatusFlags \|= FC_STATUS_MOTOR_RUN;
Line 545...	Line 545...
545		545
546	if(I2C_TransferActive)	546	if(I2C_TransferActive)
547	{	547	{
548	I2C_TransferActive = 0; // enable for the next time	548	I2C_TransferActive = 0; // enable for the next time
549	}	549	}
550	else	550	else
551	{	551	{
552	motor_write = 0;	552	motor_write = 0;
553	I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode	553	I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
554	}	554	}
Line 555...	Line 555...
555	}	555	}
Line 610...	Line 610...
610	#ifdef SWITCH_LEARNS_CAREFREE	610	#ifdef SWITCH_LEARNS_CAREFREE
611	if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;	611	if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
612	#endif	612	#endif
613	CareFree = 1;	613	CareFree = 1;
614	if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;	614	if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
615	if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] \|= DEFEKT_CAREFREE_ERR; else VersionInfo.HardwareError[0] &= ~DEFEKT_CAREFREE_ERR;	615	if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] \|= FC_ERROR0_CAREFREE; else VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE;
616	}	616	}
617	else CareFree = 0;	617	else CareFree = 0;
Line 618...	Line 618...
618		618
619	if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert	619	if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert
620	{	620	{
621	beeptime = 15000;	621	beeptime = 15000;
622	BeepMuster = 0xA400;	622	BeepMuster = 0xA400;
623	CareFree = 0;	623	CareFree = 0;
Line 624...	Line 624...
624	}	624	}
625		625
Line 626...	Line 626...
626	if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}	626	if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}
Line 659...	Line 659...
659	{	659	{
660	if(RcLostTimer) RcLostTimer--;	660	if(RcLostTimer) RcLostTimer--;
661	else	661	else
662	{	662	{
663	MotorenEin = 0;	663	MotorenEin = 0;
664	FCFlags &= ~FCFLAG_NOTLANDUNG;	664	FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING;
665	}	665	}
666	ROT_ON;	666	ROT_ON;
667	if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken	667	if(modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
668	{	668	{
669	GasMischanteil = EE_Parameter.NotGas;	669	GasMischanteil = EE_Parameter.NotGas;
670	FCFlags \|= FCFLAG_NOTLANDUNG;	670	FC_StatusFlags \|= FC_STATUS_EMERGENCY_LANDING;
671	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	671	PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
672	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;	672	PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
673	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;	673	PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
674	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;	674	PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
675	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;	675	PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
Line 680...	Line 680...
680	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	680	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
681	// Emfang gut	681	// Emfang gut
682	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	682	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
683	if(SenderOkay > 140)	683	if(SenderOkay > 140)
684	{	684	{
685	FCFlags &= ~FCFLAG_NOTLANDUNG;	685	FC_StatusFlags &= ~FC_STATUS_EMERGENCY_LANDING;
686	RcLostTimer = EE_Parameter.NotGasZeit * 50;	686	RcLostTimer = EE_Parameter.NotGasZeit * 50;
687	if(GasMischanteil > 40 && MotorenEin)	687	if(GasMischanteil > 40 && MotorenEin)
688	{	688	{
689	if(modell_fliegt < 0xffff) modell_fliegt++;	689	if(modell_fliegt < 0xffff) modell_fliegt++;
690	}	690	}
Line 696...	Line 696...
696	Mess_Integral_Gier = 0;	696	Mess_Integral_Gier = 0;
697	if(modell_fliegt == 250)	697	if(modell_fliegt == 250)
698	{	698	{
699	NeueKompassRichtungMerken = 1;	699	NeueKompassRichtungMerken = 1;
700	}	700	}
701	} else FCFlags \|= FCFLAG_FLY;	701	} else FC_StatusFlags \|= FC_STATUS_FLY;
Line 702...	Line 702...
702		702
703	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)	703	if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
704	{	704	{
705	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	705	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 737...	Line 737...
737	{	737	{
738	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();	738	if((MessLuftdruck > 950) \|\| (MessLuftdruck < 750)) SucheLuftruckOffset();
739	}	739	}
740	ServoActive = 0;	740	ServoActive = 0;
741	SetNeutral(0);	741	SetNeutral(0);
742	calibration_done = 1;	742	calibration_done = 1;
743	ServoActive = 1;	743	ServoActive = 1;
744	DDRD \|=0x80; // enable J7 -> Servo signal	744	DDRD \|=0x80; // enable J7 -> Servo signal
745	Piep(GetActiveParamSet(),120);	745	Piep(GetActiveParamSet(),120);
746	}	746	}
747	}	747	}
Line 791...	Line 791...
791	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;	791	Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
792	Mess_IntegralNick2 = IntegralNick;	792	Mess_IntegralNick2 = IntegralNick;
793	Mess_IntegralRoll2 = IntegralRoll;	793	Mess_IntegralRoll2 = IntegralRoll;
794	SummeNick = 0;	794	SummeNick = 0;
795	SummeRoll = 0;	795	SummeRoll = 0;
796	FCFlags \|= FCFLAG_START;	796	FC_StatusFlags \|= FC_STATUS_START;
797	ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;	797	ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
798	}	798	}
799	else	799	else
800	{	800	{
801	beeptime = 1500; // indicate missing calibration	801	beeptime = 1500; // indicate missing calibration
Line 824...	Line 824...
824	}	824	}
825	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	825	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
826	// neue Werte von der Funke	826	// neue Werte von der Funke
827	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	827	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 828...	Line 828...
828		828
829	if(!NewPpmData-- \|\| (FCFlags & FCFLAG_NOTLANDUNG))	829	if(!NewPpmData-- \|\| (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
830	{	830	{
831	static int stick_nick,stick_roll;	831	static int stick_nick,stick_roll;
832	ParameterZuordnung();	832	ParameterZuordnung();
833	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;	833	stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
Line 892...	Line 892...
892	{	892	{
893	MaxStickRoll = abs(StickRoll)/STICK_GAIN;	893	MaxStickRoll = abs(StickRoll)/STICK_GAIN;
894	if(MaxStickRoll > 100) MaxStickRoll = 100;	894	if(MaxStickRoll > 100) MaxStickRoll = 100;
895	}	895	}
896	else MaxStickRoll--;	896	else MaxStickRoll--;
897	if(FCFlags & FCFLAG_NOTLANDUNG) {MaxStickNick = 0; MaxStickRoll = 0;}	897	if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING) {MaxStickNick = 0; MaxStickRoll = 0;}
Line 898...	Line 898...
898		898
899	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	899	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
900	// Looping?	900	// Looping?
901	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	901	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 944...	Line 944...
944		944
945		945
946	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	946	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
947	// Bei Empfangsausfall im Flug	947	// Bei Empfangsausfall im Flug
948	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	948	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
949	if(FCFlags & FCFLAG_NOTLANDUNG)	949	if(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)
950	{	950	{
951	StickGier = 0;	951	StickGier = 0;
952	StickNick = 0;	952	StickNick = 0;
Line 1381...	Line 1381...
1381	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg	1381	tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg
1382	CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg	1382	CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
1383	LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle	1383	LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
1384	CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude	1384	CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
1385	VarioCharacter = ' ';	1385	VarioCharacter = ' ';
1386	if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))	1386	if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
1387	{	1387	{
1388	#define HEIGHT_TRIM_UP 0x01	1388	#define HEIGHT_TRIM_UP 0x01
1389	#define HEIGHT_TRIM_DOWN 0x02	1389	#define HEIGHT_TRIM_DOWN 0x02
1390	static unsigned char HeightTrimmingFlag = 0x00;	1390	static unsigned char HeightTrimmingFlag = 0x00;
Line 1404...	Line 1404...
1404	{	1404	{
1405	// alternative height control	1405	// alternative height control
1406	// PD-Control with respect to hoover point	1406	// PD-Control with respect to hoover point
1407	// the thrust loss out of horizontal attitude is compensated	1407	// the thrust loss out of horizontal attitude is compensated
1408	// the setpoint will be fine adjusted with the gas stick position	1408	// the setpoint will be fine adjusted with the gas stick position
1409	if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying	1409	if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying
1410	{ // gas stick is above hoover point	1410	{ // gas stick is above hoover point
1411	if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)	1411	if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
1412	{	1412	{
1413	if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)	1413	if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
1414	{	1414	{
Line 1439...	Line 1439...
1439	SollHoehe = HoehenWert; // update setpoint to current height	1439	SollHoehe = HoehenWert; // update setpoint to current height
1440	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;	1440	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1441	if(!StartTrigger && HoehenWert > 50)	1441	if(!StartTrigger && HoehenWert > 50)
1442	{	1442	{
1443	StartTrigger = 1;	1443	StartTrigger = 1;
1444	}	1444	}
1445	}	1445	}
1446	VarioCharacter = '=';	1446	VarioCharacter = '=';
1447	}	1447	}
1448	// Trim height set point	1448	// Trim height set point
1449	if(abs(HeightTrimming) > 512)	1449	if(abs(HeightTrimming) > 512)
Line 1489...	Line 1489...
1489	tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part	1489	tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
1490	LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense	1490	LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
1491	GasReduction = tmp_long;	1491	GasReduction = tmp_long;
1492	// ------------------------- D-Part 1: Vario Meter ----------------------------	1492	// ------------------------- D-Part 1: Vario Meter ----------------------------
1493	tmp_int = VarioMeter / 8;	1493	tmp_int = VarioMeter / 8;
1494	LIMIT_MIN_MAX(tmp_int, -127, 128);	1494	LIMIT_MIN_MAX(tmp_int, -127, 128);
1495	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter	1495	tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1496	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);	1496	LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
1497	if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint	1497	if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint
1498	else	1498	else
1499	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode	1499	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
1500	GasReduction += tmp_int;	1500	GasReduction += tmp_int;
1501	} // EOF no baro range expanding	1501	} // EOF no baro range expanding
1502	// ------------------------ D-Part 2: ACC-Z Integral ------------------------	1502	// ------------------------ D-Part 2: ACC-Z Integral ------------------------
1503	if(Parameter_Hoehe_ACC_Wirkung)	1503	if(Parameter_Hoehe_ACC_Wirkung)
1504	{	1504	{
1505	tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);	1505	tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
1506	LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);	1506	LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
1507	GasReduction += tmp_long;	1507	GasReduction += tmp_long;
1508	}	1508	}
1509	// ------------------------ D-Part 3: GpsZ ----------------------------------	1509	// ------------------------ D-Part 3: GpsZ ----------------------------------
1510	tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;	1510	tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
1511	LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);	1511	LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
1512	GasReduction += tmp_int;	1512	GasReduction += tmp_int;
1513	GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value	1513	GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
1514	// ------------------------ ----------------------------------	1514	// ------------------------ ----------------------------------
1515	HCGas -= GasReduction;	1515	HCGas -= GasReduction;
1516	// limit deviation from hoover point within the target region	1516	// limit deviation from hoover point within the target region
1517	if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero	1517	if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
1518	{	1518	{
1519	unsigned int tmp;	1519	unsigned int tmp;
1520	tmp = abs(HeightDeviation);	1520	tmp = abs(HeightDeviation);
1521	if(tmp <= 60)	1521	if(tmp <= 60)
1522	{	1522	{
1523	LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point	1523	LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point
1524	}	1524	}
1525	else	1525	else
1526	{	1526	{
1527	tmp = (tmp - 60) / 32;	1527	tmp = (tmp - 60) / 32;
1528	if(tmp > 15) tmp = 15;	1528	if(tmp > 15) tmp = 15;
1529	if(HeightDeviation > 0)	1529	if(HeightDeviation > 0)
1530	{	1530	{
1531	tmp = (HoverGasMin * (16 - tmp)) / 16;	1531	tmp = (HoverGasMin * (16 - tmp)) / 16;
Line 1551...	Line 1551...
1551	// set GasMischanteil to HeightControlGasFilter	1551	// set GasMischanteil to HeightControlGasFilter
1552	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)	1552	if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
1553	{ // old version	1553	{ // old version
1554	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas	1554	LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1555	GasMischanteil = FilterHCGas;	1555	GasMischanteil = FilterHCGas;
1556	}	1556	}
1557	else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode	1557	else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
1558	}	1558	}
1559	}// EOF height control active	1559	}// EOF height control active
1560	else // HC not active	1560	else // HC not active
1561	{	1561	{
Line 1570...	Line 1570...
1570	FilterHCGas = GasMischanteil;	1570	FilterHCGas = GasMischanteil;
1571	}	1571	}
Line 1572...	Line 1572...
1572		1572
1573	// Hover gas estimation by averaging gas control output on small z-velocities	1573	// Hover gas estimation by averaging gas control output on small z-velocities
1574	// this is done only if height contol option is selected in global config and aircraft is flying	1574	// this is done only if height contol option is selected in global config and aircraft is flying
1575	if((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_NOTLANDUNG))	1575	if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING))
1576	{	1576	{
1577	if(HoverGasFilter == 0 \|\| StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation	1577	if(HoverGasFilter == 0 \|\| StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
1578	if(StartTrigger == 1) StartTrigger = 2;	1578	if(StartTrigger == 1) StartTrigger = 2;
1579	tmp_long2 = (int32_t)GasMischanteil; // take current thrust	1579	tmp_long2 = (int32_t)GasMischanteil; // take current thrust
Line 1607...	Line 1607...
1607	else	1607	else
1608	{ // no limit	1608	{ // no limit
1609	HoverGasMin = 0;	1609	HoverGasMin = 0;
1610	HoverGasMax = 1023;	1610	HoverGasMax = 1023;
1611	}	1611	}
1612	}	1612	}
1613	else	1613	else
1614	{	1614	{
1615	StartTrigger = 0;	1615	StartTrigger = 0;
1616	HoverGasFilter = 0;	1616	HoverGasFilter = 0;
1617	HoverGas = 0;	1617	HoverGas = 0;
1618	}	1618	}
1619	}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL	1619	}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
1620	// limit gas to parameter setting	1620	// limit gas to parameter setting
1621	LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);	1621	LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
1622	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;	1622	if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;

Subversion Repositories FlightCtrl

(root)/tags/V0.82a/fc.c @ 2753 - Rev 1760 → 1765