WebSVN - FlightCtrl - Diff - Rev 1350 and 1352 - /branches/V0.88e_ACC-HH_HR



#include "main.h"
#include "eeprom.c"
#include "mymath.h"
#include "isqrt.h"

 
unsigned char h,m,s;
unsigned int BaroExpandActive = 0;
volatile unsigned int I2CTimeout = 100;
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralGierBias;

       if(!KompassSignalSchlecht && w < 25)
        {
        GierGyroFehler += fehler;
        if(NeueKompassRichtungMerken)
         {
//         beeptime = 200;
//         KompassStartwert = KompassValue;
                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
          NeueKompassRichtungMerken = 0;
         }

//    DebugOut.Analog[25] = MesswertRoll/2;
//    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
//    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
//    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
    //DebugOut.Analog[28] = I2CError;
//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
    DebugOut.Analog[30] = GPS_Nick;
    DebugOut.Analog[31] = GPS_Roll;
  }

 

             if(OCR0A < (255 - OPA_OFFSET_STEP))
                           {
                                ExpandBaro -= 1;
                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
                                beeptime = 300;
                                BaroExpandActive = 350;
                           }
                           else
                           {
                            BaroAtLowerLimit = 1;
               }

                          if(OCR0A > OPA_OFFSET_STEP)
                           {
                                ExpandBaro += 1;
                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
                                beeptime = 300;
                                BaroExpandActive = 350;
                           }
                           else
                           {
                            BaroAtUpperLimit = 1;
               }

                                        HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
                                        HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));
                                }
                                else // Gas Stick in Hoover Range
                                {
                                        if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN))
                                        {
                                                HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN);
                                                HeightTrimming = 0;
                                                SollHoehe = HoehenWert; // update setpoint to current height
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
                                        }
                                }

                           StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
                           if(StickGasHoover < 70) StickGasHoover = 70;
                           else if(StickGasHoover > 150) StickGasHoover = 150;
                       }
                                }
 
              if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
                        } //if MikroKopterFlags & MKFLAG_FLY
                        else
                        {
                         SollHoehe = HoehenWert - 400;
                         if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;

Rev 1350	Rev 1352
Line 56...	Line 56...
56	#include "main.h"	56	#include "main.h"
57	#include "eeprom.c"	57	#include "eeprom.c"
58	#include "mymath.h"	58	#include "mymath.h"
59	#include "isqrt.h"	59	#include "isqrt.h"
Line -...	Line 60...
-		60
60		61	unsigned char h,m,s;
61	unsigned char h,m,s,BaroExpandActive = 0;	62	unsigned int BaroExpandActive = 0;
62	volatile unsigned int I2CTimeout = 100;	63	volatile unsigned int I2CTimeout = 100;
63	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;	64	int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
64	int TrimNick, TrimRoll;	65	int TrimNick, TrimRoll;
65	int AdNeutralGierBias;	66	int AdNeutralGierBias;
Line 1120...	Line 1121...
1120	if(!KompassSignalSchlecht && w < 25)	1121	if(!KompassSignalSchlecht && w < 25)
1121	{	1122	{
1122	GierGyroFehler += fehler;	1123	GierGyroFehler += fehler;
1123	if(NeueKompassRichtungMerken)	1124	if(NeueKompassRichtungMerken)
1124	{	1125	{
1125	beeptime = 200;	1126	// beeptime = 200;
1126	// KompassStartwert = KompassValue;	1127	// KompassStartwert = KompassValue;
1127	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;	1128	ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
1128	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);	1129	KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
1129	NeueKompassRichtungMerken = 0;	1130	NeueKompassRichtungMerken = 0;
1130	}	1131	}
Line 1180...	Line 1181...
1180	// DebugOut.Analog[25] = MesswertRoll/2;	1181	// DebugOut.Analog[25] = MesswertRoll/2;
1181	// DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;	1182	// DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1182	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;	1183	// DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
1183	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;	1184	// DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1184	//DebugOut.Analog[28] = I2CError;	1185	//DebugOut.Analog[28] = I2CError;
1185	DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;	1186	// DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
1186	DebugOut.Analog[30] = GPS_Nick;	1187	DebugOut.Analog[30] = GPS_Nick;
1187	DebugOut.Analog[31] = GPS_Roll;	1188	DebugOut.Analog[31] = GPS_Roll;
1188	}	1189	}
Line 1189...	Line 1190...
1189		1190
Line 1249...	Line 1250...
1249	if(OCR0A < (255 - OPA_OFFSET_STEP))	1250	if(OCR0A < (255 - OPA_OFFSET_STEP))
1250	{	1251	{
1251	ExpandBaro -= 1;	1252	ExpandBaro -= 1;
1252	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down	1253	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
1253	beeptime = 300;	1254	beeptime = 300;
1254	BaroExpandActive = 250;	1255	BaroExpandActive = 350;
1255	}	1256	}
1256	else	1257	else
1257	{	1258	{
1258	BaroAtLowerLimit = 1;	1259	BaroAtLowerLimit = 1;
1259	}	1260	}
Line 1265...	Line 1266...
1265	if(OCR0A > OPA_OFFSET_STEP)	1266	if(OCR0A > OPA_OFFSET_STEP)
1266	{	1267	{
1267	ExpandBaro += 1;	1268	ExpandBaro += 1;
1268	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up	1269	OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
1269	beeptime = 300;	1270	beeptime = 300;
1270	BaroExpandActive = 250;	1271	BaroExpandActive = 350;
1271	}	1272	}
1272	else	1273	else
1273	{	1274	{
1274	BaroAtUpperLimit = 1;	1275	BaroAtUpperLimit = 1;
1275	}	1276	}
Line 1363...	Line 1364...
1363	HeightTrimmingFlag \|= HEIGHT_TRIM_DOWN;	1364	HeightTrimmingFlag \|= HEIGHT_TRIM_DOWN;
1364	HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));	1365	HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));
1365	}	1366	}
1366	else // Gas Stick in Hoover Range	1367	else // Gas Stick in Hoover Range
1367	{	1368	{
1368	if(HeightTrimmingFlag & (HEIGHT_TRIM_UP\|HEIGHT_TRIM_DOWN))	1369	if(HeightTrimmingFlag & (HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN))
1369	{	1370	{
1370	HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP\|HEIGHT_TRIM_DOWN);	1371	HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP \| HEIGHT_TRIM_DOWN);
1371	HeightTrimming = 0;	1372	HeightTrimming = 0;
1372	SollHoehe = HoehenWert; // update setpoint to current height	1373	SollHoehe = HoehenWert; // update setpoint to current height
1373	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;	1374	if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1374	}	1375	}
1375	}	1376	}
Line 1386...	Line 1387...
1386	StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;	1387	StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
1387	if(StickGasHoover < 70) StickGasHoover = 70;	1388	if(StickGasHoover < 70) StickGasHoover = 70;
1388	else if(StickGasHoover > 150) StickGasHoover = 150;	1389	else if(StickGasHoover > 150) StickGasHoover = 150;
1389	}	1390	}
1390	}	1391	}
1391		-
-		1392	if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
1392	} //if MikroKopterFlags & MKFLAG_FLY	1393	} //if MikroKopterFlags & MKFLAG_FLY
1393	else	1394	else
1394	{	1395	{
1395	SollHoehe = HoehenWert - 400;	1396	SollHoehe = HoehenWert - 400;
1396	if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;	1397	if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;

Subversion Repositories FlightCtrl

(root)/branches/V0.88e_ACC-HH_HR_MartinR/fc.c @ 2161 - Rev 1350 → 1352