Rev 1099 | Rev 1102 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1099 | Rev 1101 | ||
|---|---|---|---|
| Line 289... | Line 289... | ||
| 289 | NaviAccRoll += AdWertAccRoll; |
289 | NaviAccRoll += AdWertAccRoll; |
| 290 | NaviCntAcc++; |
290 | NaviCntAcc++; |
| 291 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
291 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
| 292 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
292 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
| 293 | ErsatzKompass += MesswertGier; |
293 | ErsatzKompass += MesswertGier; |
| 294 | //Salvo 29.12.2008 |
294 | //Salvo 30.12.2008 |
| 295 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
295 | // if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| 296 | { |
296 | { |
| 297 | GyroKomp_Int += (long)MesswertGier; |
297 | GyroKomp_Int += (long)MesswertGier; |
| 298 | GyroGier_Comp += (long)MesswertGier; |
298 | GyroGier_Comp += (long)MesswertGier; |
| 299 | } |
299 | } |
| Line 1106... | Line 1106... | ||
| 1106 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
1106 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
| 1107 | { |
1107 | { |
| 1108 | Kompass_Neuer_Wert = 0; |
1108 | Kompass_Neuer_Wert = 0; |
| 1109 | w = (abs(Mittelwert_AccNick)); |
1109 | w = (abs(Mittelwert_AccNick)); |
| 1110 | v = (abs(Mittelwert_AccRoll)); |
1110 | v = (abs(Mittelwert_AccRoll)); |
| 1111 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
1111 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT) && (Kompass_present>0)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
| 1112 | { |
1112 | { |
| 1113 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
1113 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
| 1114 | { |
1114 | { |
| 1115 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
1115 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
| 1116 | { |
1116 | { |
| Line 1180... | Line 1180... | ||
| 1180 | //GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
1180 | //GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
| 1181 | //GPS Hold Aktivieren wenn Knueppel in Ruhelage sind |
1181 | //GPS Hold Aktivieren wenn Knueppel in Ruhelage sind |
| 1182 | if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
1182 | if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold) |
| 1183 | && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
1183 | && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0)) |
| 1184 | { |
1184 | { |
| 1185 | if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv |
1185 | if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (Kompass_present>0)) //Hoehenschalter und GPS Flag aktiv |
| 1186 | { |
1186 | { |
| 1187 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
1187 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
| 1188 | else gps_cmd = GPS_CMD_REQ_HOME; |
1188 | else gps_cmd = GPS_CMD_REQ_HOME; |
| 1189 | n = GPS_CRTL(gps_cmd); |
1189 | n = GPS_CRTL(gps_cmd); |
| 1190 | } |
1190 | } |
| 1191 | else if ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv |
1191 | else if ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)&& (Kompass_present>0)) //Hoehenschalter Mittelstellung und GPS Flag aktiv |
| 1192 | { |
1192 | { |
| 1193 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
1193 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
| 1194 | else gps_cmd = GPS_CMD_REQ_HOLD; |
1194 | else gps_cmd = GPS_CMD_REQ_HOLD; |
| 1195 | n= GPS_CRTL(gps_cmd); |
1195 | n= GPS_CRTL(gps_cmd); |
| 1196 | } |
1196 | } |
| Line 1244... | Line 1244... | ||
| 1244 | // Salvo End |
1244 | // Salvo End |
| 1245 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1245 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| 1246 | if(w > 0) |
1246 | if(w > 0) |
| 1247 | { |
1247 | { |
| 1248 | // Salvo Kompasssteuerung ********************** |
1248 | // Salvo Kompasssteuerung ********************** |
| 1249 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
1249 | if ((magkompass_ok > 0) && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
| 1250 | // Salvo End |
1250 | // Salvo End |
| 1251 | } |
1251 | } |
| Line 1252... | Line 1252... | ||
| 1252 | 1252 | ||
| Line 1293... | Line 1293... | ||
| 1293 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
1293 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
| 1294 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
1294 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
| 1295 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
1295 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
| 1296 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
1296 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
| 1297 | // DebugOut.Analog[29] = magkompass_ok; |
1297 | // DebugOut.Analog[29] = magkompass_ok; |
| - | 1298 | // DebugOut.Analog[29] = Kompass_present; |
|
| Line 1298... | Line 1299... | ||
| 1298 | 1299 | ||
| 1299 | 1300 | ||
| 1300 | DebugOut.Analog[30] = GPS_Nick; |
1301 | DebugOut.Analog[30] = GPS_Nick; |