Rev 1600 | Rev 1626 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1600 | Rev 1622 | ||
---|---|---|---|
Line 52... | Line 52... | ||
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 55... | Line 55... | ||
55 | 55 | ||
56 | #include "main.h" |
- | |
57 | #include "eeprom.c" |
56 | #include "main.h" |
58 | #include "mymath.h" |
57 | #include "mymath.h" |
Line 59... | Line 58... | ||
59 | #include "isqrt.h" |
58 | #include "isqrt.h" |
60 | 59 | ||
61 | unsigned char h,m,s; |
60 | unsigned char h,m,s; |
62 | unsigned int BaroExpandActive = 0; |
61 | unsigned int BaroExpandActive = 0; |
63 | volatile unsigned int I2CTimeout = 100; |
62 | volatile unsigned int I2CTimeout = 100; |
64 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
- | |
65 | int TrimNick, TrimRoll; |
63 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
66 | int AdNeutralGierBias; |
64 | int TrimNick, TrimRoll; |
67 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
68 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
69 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
67 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
Line 95... | Line 93... | ||
95 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
93 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
96 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
94 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
97 | volatile unsigned char SenderOkay = 0; |
95 | volatile unsigned char SenderOkay = 0; |
98 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
96 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | char MotorenEin = 0; |
97 | char MotorenEin = 0; |
- | 98 | unsigned char RequiredMotors = 0; |
|
100 | long HoehenWert = 0; |
99 | long HoehenWert = 0; |
101 | long SollHoehe = 0; |
100 | long SollHoehe = 0; |
102 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
101 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
103 | //float Ki = FAKTOR_I; |
102 | //float Ki = FAKTOR_I; |
104 | int Ki = 10300 / 33; |
103 | int Ki = 10300 / 33; |
Line 147... | Line 146... | ||
147 | unsigned char Parameter_NaviOperatingRadius; |
146 | unsigned char Parameter_NaviOperatingRadius; |
148 | unsigned char Parameter_NaviWindCorrection; |
147 | unsigned char Parameter_NaviWindCorrection; |
149 | unsigned char Parameter_NaviSpeedCompensation; |
148 | unsigned char Parameter_NaviSpeedCompensation; |
150 | unsigned char Parameter_ExternalControl; |
149 | unsigned char Parameter_ExternalControl; |
151 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
152 | struct mk_param_struct EE_Parameter; |
- | |
- | 151 | ||
153 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
152 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
154 | int MaxStickNick = 0,MaxStickRoll = 0; |
153 | int MaxStickNick = 0,MaxStickRoll = 0; |
155 | unsigned int modell_fliegt = 0; |
154 | unsigned int modell_fliegt = 0; |
156 | volatile unsigned char FCFlags = 0; |
155 | volatile unsigned char FCFlags = 0; |
157 | long GIER_GRAD_FAKTOR = 1291; |
156 | long GIER_GRAD_FAKTOR = 1291; |
Line 161... | Line 160... | ||
161 | 160 | ||
162 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
161 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
163 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
162 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
Line -... | Line 163... | ||
- | 163 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
|
164 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
164 | |
165 | 165 | ||
166 | int MotorSmoothing(int neu, int alt) |
166 | int MotorSmoothing(int neu, int alt) |
167 | { |
167 | { |
168 | int motor; |
168 | int motor; |
Line 182... | Line 182... | ||
182 | Delay_ms(dauer * 2); |
182 | Delay_ms(dauer * 2); |
183 | } |
183 | } |
184 | } |
184 | } |
Line 185... | Line 185... | ||
185 | 185 | ||
- | 186 | //############################################################################ |
|
- | 187 | // Messwerte beim Ermitteln der Nullage |
|
- | 188 | void CalibrierMittelwert(void) |
|
- | 189 | //############################################################################ |
|
- | 190 | { |
|
- | 191 | unsigned char i; |
|
- | 192 | if(PlatinenVersion == 13) SucheGyroOffset(); |
|
- | 193 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 194 | ANALOG_OFF; |
|
- | 195 | MesswertNick = AdWertNick; |
|
- | 196 | MesswertRoll = AdWertRoll; |
|
- | 197 | MesswertGier = AdWertGier; |
|
- | 198 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
|
- | 199 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
|
- | 200 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
|
- | 201 | // ADC einschalten |
|
- | 202 | ANALOG_ON; |
|
- | 203 | for(i=0;i<8;i++) |
|
- | 204 | { |
|
- | 205 | int tmp; |
|
- | 206 | tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110; |
|
- | 207 | LIMIT_MIN_MAX(tmp, 0, 255); |
|
- | 208 | if(Poti[i] > tmp) Poti[i]--; else if(Poti[i] < tmp) Poti[i]++; |
|
- | 209 | } |
|
- | 210 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
|
- | 211 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
|
- | 212 | } |
|
- | 213 | ||
186 | //############################################################################ |
214 | //############################################################################ |
187 | // Nullwerte ermitteln |
215 | // Nullwerte ermitteln |
188 | void SetNeutral(void) |
216 | void SetNeutral(unsigned char AccAdjustment) |
189 | //############################################################################ |
217 | //############################################################################ |
190 | { |
218 | { |
191 | unsigned char i; |
219 | unsigned char i; |
- | 220 | unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
|
192 | unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
221 | |
193 | HEF4017R_ON; |
222 | HEF4017R_ON; |
194 | NeutralAccX = 0; |
223 | NeutralAccX = 0; |
195 | NeutralAccY = 0; |
224 | NeutralAccY = 0; |
- | 225 | NeutralAccZ = 0; |
|
196 | NeutralAccZ = 0; |
226 | |
197 | AdNeutralNick = 0; |
227 | AdNeutralNick = 0; |
198 | AdNeutralRoll = 0; |
228 | AdNeutralRoll = 0; |
199 | AdNeutralGier = 0; |
- | |
- | 229 | AdNeutralGier = 0; |
|
200 | AdNeutralGierBias = 0; |
230 | |
201 | Parameter_AchsKopplung1 = 0; |
231 | Parameter_AchsKopplung1 = 0; |
- | 232 | Parameter_AchsKopplung2 = 0; |
|
202 | Parameter_AchsKopplung2 = 0; |
233 | |
- | 234 | ExpandBaro = 0; |
|
203 | ExpandBaro = 0; |
235 | |
- | 236 | CalibrierMittelwert(); |
|
204 | CalibrierMittelwert(); |
237 | |
- | 238 | Delay_ms_Mess(100); |
|
205 | Delay_ms_Mess(100); |
239 | |
- | 240 | CalibrierMittelwert(); |
|
206 | CalibrierMittelwert(); |
241 | |
207 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
242 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
208 | { |
243 | { |
209 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
244 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
210 | } |
245 | } |
Line 217... | Line 252... | ||
217 | roll_neutral += AdWertRoll; |
252 | roll_neutral += AdWertRoll; |
218 | } |
253 | } |
219 | AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
254 | AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
220 | AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
255 | AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
221 | AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
256 | AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
222 | AdNeutralGierBias = AdNeutralGier; |
- | |
- | 257 | ||
223 | StartNeutralRoll = AdNeutralRoll; |
258 | StartNeutralRoll = AdNeutralRoll; |
224 | StartNeutralNick = AdNeutralNick; |
259 | StartNeutralNick = AdNeutralNick; |
- | 260 | ||
225 | if(eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK)) > 4) |
261 | if(AccAdjustment) |
226 | { |
262 | { |
227 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
263 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
228 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
264 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
229 | NeutralAccZ = Aktuell_az; |
265 | NeutralAccZ = Aktuell_az; |
- | 266 | ||
- | 267 | // Save ACC neutral settings to eeprom |
|
- | 268 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccY); |
|
- | 269 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccX); |
|
- | 270 | SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ); |
|
230 | } |
271 | } |
231 | else |
272 | else |
232 | { |
273 | { |
- | 274 | // restore from eeprom |
|
233 | NeutralAccX = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK+1)); |
275 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
234 | NeutralAccY = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL+1)); |
276 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
- | 277 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
|
- | 278 | // strange settings? |
|
235 | NeutralAccZ = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_Z)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_Z+1)); |
279 | if((NeutralAccX > 2048) || (NeutralAccY > 2048) || (NeutralAccZ > 1024)) |
- | 280 | { |
|
- | 281 | printf("\n\rACC not calibrated!\r\n"); |
|
- | 282 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
|
- | 283 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
|
- | 284 | NeutralAccZ = Aktuell_az; |
|
- | 285 | } |
|
236 | } |
286 | } |
Line 237... | Line 287... | ||
237 | 287 | ||
238 | MesswertNick = 0; |
288 | MesswertNick = 0; |
239 | MesswertRoll = 0; |
289 | MesswertRoll = 0; |
Line 284... | Line 334... | ||
284 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
334 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
285 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
335 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
286 | signed long winkel_nick, winkel_roll; |
336 | signed long winkel_nick, winkel_roll; |
287 | unsigned char i; |
337 | unsigned char i; |
288 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
338 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
289 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
- | |
290 | MesswertNick = (signed int) AdWertNickFilter / 8; |
339 | MesswertNick = (signed int) AdWertNickFilter / 8; |
291 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
340 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
292 | RohMesswertNick = MesswertNick; |
341 | RohMesswertNick = MesswertNick; |
293 | RohMesswertRoll = MesswertRoll; |
342 | RohMesswertRoll = MesswertRoll; |
Line 438... | Line 487... | ||
438 | } |
487 | } |
439 | } |
488 | } |
440 | } |
489 | } |
Line 441... | Line 490... | ||
441 | 490 | ||
442 | //############################################################################ |
- | |
443 | // Messwerte beim Ermitteln der Nullage |
- | |
444 | void CalibrierMittelwert(void) |
- | |
445 | //############################################################################ |
- | |
446 | { |
- | |
447 | unsigned char i; |
- | |
448 | if(PlatinenVersion == 13) SucheGyroOffset(); |
- | |
449 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
- | |
450 | ANALOG_OFF; |
- | |
451 | MesswertNick = AdWertNick; |
- | |
452 | MesswertRoll = AdWertRoll; |
- | |
453 | MesswertGier = AdWertGier; |
- | |
454 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
- | |
455 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
- | |
456 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
- | |
457 | // ADC einschalten |
- | |
458 | ANALOG_ON; |
- | |
459 | for(i=0;i<8;i++) |
- | |
460 | { |
- | |
461 | int tmp; |
- | |
462 | tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110; |
- | |
463 | LIMIT_MIN_MAX(tmp, 0, 255); |
- | |
464 | if(Poti[i] > tmp) Poti[i]--; else if(Poti[i] < tmp) Poti[i]++; |
- | |
465 | } |
- | |
466 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
- | |
467 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
- | |
468 | } |
- | |
469 | - | ||
470 | //############################################################################ |
491 | //############################################################################ |
471 | // Senden der Motorwerte per I2C-Bus |
492 | // Senden der Motorwerte per I2C-Bus |
472 | void SendMotorData(void) |
493 | void SendMotorData(void) |
473 | //############################################################################ |
494 | //############################################################################ |
474 | { |
495 | { |
Line 558... | Line 579... | ||
558 | static long IntegralFehlerNick = 0; |
579 | static long IntegralFehlerNick = 0; |
559 | static long IntegralFehlerRoll = 0; |
580 | static long IntegralFehlerRoll = 0; |
560 | static unsigned int RcLostTimer; |
581 | static unsigned int RcLostTimer; |
561 | static unsigned char delay_neutral = 0; |
582 | static unsigned char delay_neutral = 0; |
562 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
583 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
- | 584 | static unsigned char calibration_done = 0; |
|
563 | static char TimerWerteausgabe = 0; |
585 | static char TimerWerteausgabe = 0; |
564 | static char NeueKompassRichtungMerken = 0; |
586 | static char NeueKompassRichtungMerken = 0; |
565 | static long ausgleichNick, ausgleichRoll; |
587 | static long ausgleichNick, ausgleichRoll; |
566 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
588 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
567 | unsigned char i; |
589 | unsigned char i; |
Line 641... | Line 663... | ||
641 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; |
663 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1; |
642 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
664 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2; |
643 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
665 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
644 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
666 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
645 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
667 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
646 | SetActiveParamSetNumber(setting); // aktiven Datensatz merken |
668 | SetActiveParamSet(setting); // aktiven Datensatz merken |
647 | } |
669 | } |
648 | // else |
670 | // else |
649 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
671 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) |
650 | { |
672 | { |
651 | WinkelOut.CalcState = 1; |
673 | WinkelOut.CalcState = 1; |
652 | beeptime = 1000; |
674 | beeptime = 1000; |
653 | } |
675 | } |
654 | else |
676 | else |
655 | { |
677 | { |
656 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
678 | ParamSet_ReadFromEEProm(GetActiveParamSet()); |
657 | LipoDetection(0); |
679 | LipoDetection(0); |
658 | LIBFC_ReceiverInit(); |
680 | LIBFC_ReceiverInit(); |
659 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
681 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
660 | { |
682 | { |
661 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
683 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
662 | } |
684 | } |
663 | ServoActive = 0; |
685 | ServoActive = 0; |
664 | SetNeutral(); |
686 | SetNeutral(0); |
- | 687 | calibration_done = 1; |
|
665 | ServoActive = 1; |
688 | ServoActive = 1; |
666 | DDRD |=0x80; // enable J7 -> Servo signal |
689 | DDRD |=0x80; // enable J7 -> Servo signal |
667 | Piep(GetActiveParamSetNumber(),120); |
690 | Piep(GetActiveParamSet(),120); |
668 | } |
691 | } |
669 | } |
692 | } |
670 | } |
693 | } |
671 | else |
694 | else |
672 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
695 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
673 | { |
696 | { |
674 | if(++delay_neutral > 200) // nicht sofort |
697 | if(++delay_neutral > 200) // nicht sofort |
675 | { |
698 | { |
676 | GRN_OFF; |
699 | GRN_OFF; |
677 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK),0xff); // Werte löschen |
- | |
678 | MotorenEin = 0; |
700 | MotorenEin = 0; |
679 | delay_neutral = 0; |
701 | delay_neutral = 0; |
680 | modell_fliegt = 0; |
702 | modell_fliegt = 0; |
681 | SetNeutral(); |
703 | SetNeutral(1); |
682 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK),NeutralAccX / 256); // ACC-NeutralWerte speichern |
- | |
683 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK+1),NeutralAccX % 256); // ACC-NeutralWerte speichern |
- | |
684 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL),NeutralAccY / 256); |
704 | calibration_done = 1; |
685 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL+1),NeutralAccY % 256); |
- | |
686 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_Z),(int)NeutralAccZ / 256); |
- | |
687 | eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_Z+1),(int)NeutralAccZ % 256); |
- | |
688 | Piep(GetActiveParamSetNumber(),120); |
705 | Piep(GetActiveParamSet(),120); |
689 | } |
706 | } |
690 | } |
707 | } |
691 | else delay_neutral = 0; |
708 | else delay_neutral = 0; |
692 | } |
709 | } |
693 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
710 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 704... | Line 721... | ||
704 | // Einschalten |
721 | // Einschalten |
705 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
722 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
706 | if(++delay_einschalten > 200) |
723 | if(++delay_einschalten > 200) |
707 | { |
724 | { |
708 | delay_einschalten = 0; |
725 | delay_einschalten = 0; |
- | 726 | if(calibration_done) |
|
- | 727 | { |
|
709 | modell_fliegt = 1; |
728 | modell_fliegt = 1; |
710 | MotorenEin = 1; |
729 | MotorenEin = 1; |
711 | sollGier = 0; |
730 | sollGier = 0; |
712 | Mess_Integral_Gier = 0; |
731 | Mess_Integral_Gier = 0; |
713 | Mess_Integral_Gier2 = 0; |
732 | Mess_Integral_Gier2 = 0; |
714 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
733 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
715 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
734 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
716 | Mess_IntegralNick2 = IntegralNick; |
735 | Mess_IntegralNick2 = IntegralNick; |
717 | Mess_IntegralRoll2 = IntegralRoll; |
736 | Mess_IntegralRoll2 = IntegralRoll; |
718 | SummeNick = 0; |
737 | SummeNick = 0; |
719 | SummeRoll = 0; |
738 | SummeRoll = 0; |
720 | FCFlags |= FCFLAG_START; |
739 | FCFlags |= FCFLAG_START; |
- | 740 | } |
|
- | 741 | else |
|
- | 742 | { |
|
- | 743 | beeptime = 1500; // indicate missing calibration |
|
- | 744 | } |
|
721 | } |
745 | } |
722 | } |
746 | } |
723 | else delay_einschalten = 0; |
747 | else delay_einschalten = 0; |
724 | } |
748 | } |
725 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
749 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 984... | Line 1008... | ||
984 | Mess_IntegralNick2 -= IntegralFehlerNick; |
1008 | Mess_IntegralNick2 -= IntegralFehlerNick; |
985 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
1009 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
Line 986... | Line 1010... | ||
986 | 1010 | ||
987 | if(EE_Parameter.Driftkomp) |
1011 | if(EE_Parameter.Driftkomp) |
988 | { |
1012 | { |
989 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
1013 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; } |
990 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
1014 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; } |
991 | } |
1015 | } |
Line 992... | Line 1016... | ||
992 | GierGyroFehler = 0; |
1016 | GierGyroFehler = 0; |
Line 1615... | Line 1639... | ||
1615 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1639 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1616 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1640 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1617 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1641 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1618 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1642 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1619 | tmp_int = tmp_motorwert[i] / STICK_GAIN; |
1643 | tmp_int = tmp_motorwert[i] / STICK_GAIN; |
1620 | CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1644 | LIMIT_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1621 | Motor[i].SetPoint = tmp_int; |
1645 | Motor[i].SetPoint = tmp_int; |
1622 | } |
1646 | } |
1623 | else Motor[i].SetPoint = 0; |
1647 | else Motor[i].SetPoint = 0; |
1624 | } |
1648 | } |
1625 | } |
1649 | } |