51,6 → 51,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 9.11.2008 |
/* |
Driftkompensation fuer Gyros verbessert |
Linearsensor optional mit fixem Neutralwert |
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion |
*/ |
|
#include "main.h" |
#include "eeprom.c" |
85,6 → 91,25 |
long ErsatzKompass; |
int ErsatzKompassInGrad; // Kompasswert in Grad |
int GierGyroFehler = 0; |
//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 15.12.2007 Ersatzkompass und Giergyrokompensation |
long GyroKomp_Int; |
long int GyroGier_Comp; |
int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
short int cnt_stickgier_zero =0; |
int gyrogier_kompass; |
//Salvo End |
|
//Salvo 2.1.2008 Allgemeine Debugvariablen |
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
//Salvo End |
float GyroFaktor; |
float IntegralFaktor; |
volatile int DiffNick,DiffRoll; |
132,6 → 157,7 |
unsigned char Parameter_NaviGpsI; |
unsigned char Parameter_NaviGpsD; |
unsigned char Parameter_NaviGpsACC; |
unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008 |
unsigned char Parameter_NaviOperatingRadius; |
unsigned char Parameter_NaviWindCorrection; |
unsigned char Parameter_NaviSpeedCompensation; |
142,6 → 168,13 |
unsigned int modell_fliegt = 0; |
unsigned char MikroKopterFlags = 0; |
|
//Salvo 2.1.2008 Allgemeine Debugvariablen |
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; |
//Salvo End |
|
|
|
|
void Piep(unsigned char Anzahl) |
{ |
while(Anzahl--) |
157,7 → 190,10 |
void SetNeutral(void) |
//############################################################################ |
{ |
NeutralAccX = 0; |
// Salvo 9.12.2007 |
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
// Salvo End |
NeutralAccX = 0; |
NeutralAccY = 0; |
NeutralAccZ = 0; |
AdNeutralNick = 0; |
220,6 → 256,13 |
FromNaviCtrl_Value.Kalman_K = -1; |
FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16; |
FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
|
//Salvo 13.10.2007 Ersatzkompass und Gas |
GyroKomp_Int = KompassValue * GIER_GRAD_FAKTOR; //Neu ab 15.10.2008 |
// gas_mittel = 30; |
// gas_actual = gas_mittel; |
// Salvo End |
|
} |
|
//############################################################################ |
234,7 → 277,7 |
MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
|
//DebugOut.Analog[26] = MesswertNick; |
DebugOut.Analog[28] = MesswertRoll; |
//DebugOut.Analog[28] = MesswertRoll; |
|
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
248,6 → 291,10 |
IntegralAccZ += Aktuell_az - NeutralAccZ; |
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
ErsatzKompass += MesswertGier; |
//Salvo 12.11.2007 |
GyroKomp_Int += (long)MesswertGier; |
GyroGier_Comp += (long)MesswertGier; |
//Salvo End |
Mess_Integral_Gier += MesswertGier; |
// Mess_Integral_Gier2 += MesswertGier; |
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
438,12 → 485,12 |
CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
|
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255); |
CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255); |
CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255); |
CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255); |
CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255); |
CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255); |
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255); |
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255); |
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255); |
475,9 → 522,17 |
static char TimerWerteausgabe = 0; |
static char NeueKompassRichtungMerken = 0; |
static long ausgleichNick, ausgleichRoll; |
|
Mittelwert(); |
|
|
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; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
551,6 → 606,7 |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetNeutral(); |
MotorenEin = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
579,6 → 635,13 |
} |
SetNeutral(); |
Piep(GetActiveParamSetNumber()); |
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 = 1000; |
Delay_ms(500); |
} |
} |
} |
} |
617,6 → 680,10 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
int n; |
// Salvo 9.12.2007 |
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten |
// Salvo End |
delay_einschalten = 200; |
modell_fliegt = 1; |
MotorenEin = 1; |
629,6 → 696,8 |
Mess_IntegralRoll2 = IntegralRoll; |
SummeNick = 0; |
SummeRoll = 0; |
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden |
|
MikroKopterFlags |= FLAG_START; |
} |
} |
641,6 → 710,10 |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
// Salvo 9.12.2007 |
RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten |
// Salvo End |
|
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
897,10 → 970,15 |
if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
|
DebugOut.Analog[22] = MittelIntegralRoll / 26; |
//DebugOut.Analog[22] = MittelIntegralRoll / 26; |
//DebugOut.Analog[24] = GierGyroFehler; |
GierGyroFehler = 0; |
|
//Salvo Ersatzkompass Ueberlauf korrigieren |
if (GyroKomp_Int >= ((long)360 * GIER_GRAD_FAKTOR)) GyroKomp_Int = GyroKomp_Int - (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007 |
ROT_OFF; |
// Salvo End |
|
/*DebugOut.Analog[17] = IntegralAccNick / 26; |
DebugOut.Analog[18] = IntegralAccRoll / 26; |
1015,8 → 1093,118 |
ZaehlMessungen = 0; |
} |
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
{ |
Kompass_Neuer_Wert = 0; |
w = (abs(Mittelwert_AccNick)); |
v = (abs(Mittelwert_AccRoll)); |
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
{ |
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
{ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
{ |
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da |
{ |
w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
if (v <-180) v +=360; // Uberlaufkorrektur |
if (v > 180) v -=360; // Uberlaufkorrektur |
|
v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler |
|
#define GIER_COMP_MAX 4 |
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX; |
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX; |
if (abs(w) > 1) |
{ |
GyroGier_Comp = 0; |
gyrogier_kompass = KompassValue; // Kompasswert merken |
AdNeutralGier -= v; |
} |
} |
} |
else |
{ |
gyrogier_kompass = KompassValue; // Kompasswert merken |
cnt_stickgier_zero = 0; |
GyroGier_Comp = 0; |
} |
|
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR; |
|
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 + 360L; |
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern |
} |
} |
else //Kompassfehler |
{ |
magkompass_ok = 0; |
GyroGier_Comp = 0; |
} |
Kompass_Value_Old = KompassValue; |
} |
// 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 ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
{ |
if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv |
{ |
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 ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv |
{ |
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 // GPS komplett aus |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
} |
else |
{ |
if (gps_cmd != GPS_CMD_STOP) |
{ |
gps_cmd = GPS_CMD_STOP; |
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden |
} |
} |
if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12) LED_J16_OFF; //led im GPS Mode schnell blinken lassen |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1040,53 → 1228,27 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
|
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
{ |
int w,v,r,fehler,korrektur; |
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if(v > w) w = v; // grösste Neigung ermitteln |
korrektur = w / 8 + 1; |
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
if(NeueKompassRichtungMerken) |
{ |
fehler = 0; |
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
} |
if(!KompassSignalSchlecht && w < 25) |
{ |
GierGyroFehler += fehler; |
if(NeueKompassRichtungMerken) |
{ |
beeptime = 200; |
// KompassStartwert = KompassValue; |
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
NeueKompassRichtungMerken = 0; |
} |
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
ErsatzKompass += (fehler * 8) / korrektur; |
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
// 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(!KompassSignalSchlecht) |
{ |
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180; |
// r = KompassRichtung; |
v = (r * w) / v; // nach Kompass ausrichten |
w = 3 * Parameter_KompassWirkung; |
if(v > w) v = w; // Begrenzen |
else |
if(v < -w) v = -w; |
Mess_Integral_Gier += v; |
} |
if(KompassSignalSchlecht) KompassSignalSchlecht--; |
} |
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Salvo Kompasssteuerung ********************** |
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
// Salvo End |
} |
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugwerte zuordnen |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1093,6 → 1255,11 |
if(!TimerWerteausgabe--) |
{ |
TimerWerteausgabe = 24; |
// Salvo 13.12.2007 Beleuchtung steuern |
if (!(beeptime & BeepMuster)) LED_J16_FLASH; |
else if (MotorenEin) LED_J16_ON; |
else LED_J16_OFF; |
// Salvo End |
|
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
1100,25 → 1267,34 |
DebugOut.Analog[3] = Mittelwert_AccRoll; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert; |
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
// DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
|
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
// DebugOut.Analog[10] = SenderOkay; |
DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR; |
//DebugOut.Analog[16] = Mittelwert_AccHoch; |
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
DebugOut.Analog[19] = WinkelOut.CalcState; |
DebugOut.Analog[20] = ServoValue; |
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
// DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
// DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
// DebugOut.Analog[19] = WinkelOut.CalcState; |
// DebugOut.Analog[20] = ServoValue; |
|
|
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128; |
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++; |
DebugOut.Analog[23] = debug_gp_0; |
DebugOut.Analog[24] = debug_gp_1; |
DebugOut.Analog[25] = debug_gp_2; |
DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
|
DebugOut.Analog[30] = GPS_Nick; |
DebugOut.Analog[31] = GPS_Roll; |
|
|
/* DebugOut.Analog[16] = motor_rx[0]; |
DebugOut.Analog[17] = motor_rx[1]; |
DebugOut.Analog[18] = motor_rx[2]; |