52,6 → 52,12 |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007 |
/* |
Driftkompensation fuer Gyros verbessert |
Linearsensor mit fixem Neutralwert |
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion |
*/ |
|
#include "main.h" |
#include "eeprom.c" |
80,6 → 86,19 |
unsigned char HoehenReglerAktiv = 0; |
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
|
//Salvo 12.10.2007 |
uint8_t magkompass_ok=0; |
uint8_t gps_cmd = GPS_CMD_STOP; |
static int ubat_cnt =0; |
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
int w,v; |
//Salvo End |
|
//Salvo 2.9.2007 Ersatzkompass |
volatile long GyroKomp_Int; |
volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
// Salvo End |
|
float GyroFaktor; |
float IntegralFaktor; |
|
103,7 → 122,7 |
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
unsigned char Parameter_Gyro_P = 50; // Wert : 10-250 |
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
140,6 → 159,7 |
AdNeutralGier = 0; |
Parameter_AchsKopplung1 = 0; |
Parameter_AchsGegenKopplung1 = 0; |
|
CalibrierMittelwert(); |
Delay_ms_Mess(100); |
CalibrierMittelwert(); |
153,8 → 173,19 |
AdNeutralGier= AdWertGier; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralNick = AdNeutralNick; |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
|
// Salvo 1.9.2007 ACC mit festen neutralwerten ***** |
if (ACC_NEUTRAL_FIXED > 0) |
{ |
NeutralAccX = ACC_NICK_NEUTRAL; |
NeutralAccY = ACC_ROLL_NEUTRAL; |
} |
else |
{ // Automatisch bei Offsetabgleich ermitteln |
NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
} |
// Salvo End |
NeutralAccZ = Aktuell_az; |
|
Mess_IntegralNick = 0; |
173,6 → 204,12 |
beeptime = 50; |
Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
|
//Salvo 13.10.2007 Ersatzkompass und Gas |
GyroKomp_Int = KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007 |
gas_mittel = 30; |
gas_actual = gas_mittel; |
// Salvo End |
} |
|
//############################################################################ |
193,6 → 230,9 |
IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
IntegralAccZ += Aktuell_az - 704;//NeutralAccZ; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
//Salvo 12.11.2007 |
GyroKomp_Int += MesswertGier; |
//Salvo End |
Mess_Integral_Gier += MesswertGier; |
Mess_Integral_Gier2 += MesswertGier; |
// Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
400,15 → 440,55 |
static long ausgleichNick, ausgleichRoll; |
|
Mittelwert(); |
//****** GPS Daten holen *************** |
short int n; |
if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout) |
n = Get_Rel_Position(); |
if (n == 0) |
{ |
ROT_ON; //led blitzen lassen |
} |
//******PROVISORISCH*************** |
GRN_ON; |
|
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
// und dieser dann langsam zwangsweise reduziert |
if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
{ |
if (ubat_cnt > 700) |
{ |
ubat_cnt = 0; |
if (gas_actual > ((gas_mittel*12)/15)) gas_actual--; |
} |
else ubat_cnt++; |
if (GasMischanteil > gas_actual) GasMischanteil = gas_actual; |
} |
else //Falls UBAT wieder ok ist |
{ |
if (ubat_cnt > 1000) |
{ |
gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern |
gas_actual = GasMischanteil; |
} |
else |
{ |
ubat_cnt++; |
if ((ubat_cnt % 10) == 0) |
{ |
if (gas_actual < GasMischanteil) gas_actual++; |
else gas_actual = GasMischanteil; |
} |
} |
GasMischanteil = gas_actual; |
} |
// Salvo End |
if(GasMischanteil < 0) GasMischanteil = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// Empfang schlecht |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
467,6 → 547,7 |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetNeutral(); |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
480,14 → 561,25 |
if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
} |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
|
|
|
|
ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
SetNeutral(); |
|
Piep(GetActiveParamSetNumber()); |
} |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
{ |
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
} |
GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
if (gps_home_position.status > 0 ) |
{ |
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
beeptime = 2000; |
Delay_ms(500); |
} |
} |
} |
else delay_neutral = 0; |
} |
641,6 → 733,7 |
} |
|
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
731,7 → 824,13 |
// IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
// IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
|
//Salvo Ersatzkompass Ueberlauf korrigieren |
if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007 |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007 |
ROT_OFF; |
// Salvo End |
|
/* |
DebugOut.Analog[30] = I_LageRoll * 10; |
DebugOut.Analog[17] = IntegralAccNick / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
741,6 → 840,7 |
DebugOut.Analog[22] = MittelIntegralRoll / 26; |
DebugOut.Analog[28] = ausgleichNick; |
DebugOut.Analog[29] = ausgleichRoll; |
*/ |
|
#define FEHLER_LIMIT (ABGLEICH_ANZAHL/2) |
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4) |
776,7 → 876,7 |
|
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
cnt = 1;// + labs(IntegralFehlerRoll) / 4096; |
ausgleichRoll = 0; |
ausgleichRoll = 0; |
if(IntegralFehlerRoll > FEHLER_LIMIT2) |
{ |
if(last_r_p) |
830,29 → 930,87 |
if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
|
// Salvo Ersatzkompass 26.9.2007 ********************** |
if ((Kompass_Neuer_Wert > 0)) |
{ |
Kompass_Neuer_Wert = 0; |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alles ok |
{ |
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
{ |
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
w = KompassValue - GyroKomp_Int; |
if ((w > 0) && (w < 180)) |
{ |
++GyroKomp_Int; |
} |
else if ((w > 0) && (w >= 180)) |
{ |
--GyroKomp_Int; |
} |
else if ((w < 0) && (w >= -180)) |
{ |
--GyroKomp_Int; |
} |
else if ((w < 0) && (w < -180)) |
{ |
++GyroKomp_Int; |
} |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360; |
GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
} |
} |
else magkompass_ok = 0; |
} |
// Salvo End ************************* |
|
// Salvo 6.10.2007 |
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(StickRoll) < GPS_STICK_HOLDOFF) && (abs(StickNick) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0)) |
{ |
if (Parameter_MaxHoehe > 200) |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOME; |
n = GPS_CRTL(gps_cmd); |
} |
else |
{ |
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
else gps_cmd = GPS_CMD_REQ_HOLD; |
n= GPS_CRTL(gps_cmd); |
} |
} |
else (n= GPS_CRTL(GPS_CMD_STOP)); //GPS Lageregelung beenden |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0)) |
{ |
int w,v; |
static int SignalSchlecht = 0; |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
if(v > w) w = v; // grösste Neigung ermitteln |
|
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
{ |
KompassStartwert = KompassValue; |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
// Salvo 13.9.2007 |
w=0; |
// Salvo End |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
if(SignalSchlecht) SignalSchlecht--; |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
// Salvo End |
} |
else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
|
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
870,10 → 1028,25 |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[16] = Mittelwert_AccHoch; |
DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
// Diverse parameter Debugging |
DebugOut.Analog[16] = dataset_cnt; |
DebugOut.Analog[17] = UBat; |
DebugOut.Analog[18] = MesswertNick; |
DebugOut.Analog[19] = MesswertRoll; |
DebugOut.Analog[20] = MesswertGier; |
DebugOut.Analog[21] = StickNick; |
DebugOut.Analog[22] = StickRoll; |
DebugOut.Analog[23] = StickGier; |
|
|
// GPS Debugging |
DebugOut.Analog[26] = gps_rel_act_position.utm_east; |
DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
DebugOut.Analog[29] = gps_sub_state+(20*gps_cmd); |
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |