Rev 1232 | Rev 1268 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1232 | Rev 1254 | ||
---|---|---|---|
Line 165... | Line 165... | ||
165 | sei(); |
165 | sei(); |
Line 166... | Line 166... | ||
166 | 166 | ||
167 | printf("\n\r==================================="); |
167 | printf("\n\r==================================="); |
168 | printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a'); |
168 | printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a'); |
169 | if(UCSR1A == 0x20 && UCSR1C == 0x06) // initial Values for 644P |
169 | if(UCSR1A == 0x20 && UCSR1C == 0x06) // initial Values for 644P |
170 | { |
170 | { |
171 | Uart1Init(); |
171 | Uart1Init(); |
172 | } |
172 | } |
173 | GRN_ON; |
173 | GRN_ON; |
Line 174... | Line 174... | ||
174 | ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes |
174 | ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes |
175 | 175 | ||
176 | if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && // Check Revision in the first Byte |
176 | if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && // Check Revision in the first Byte |
177 | (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff)) // Settings reset via Koptertool |
177 | (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff)) // Settings reset via Koptertool |
178 | { |
178 | { |
179 | unsigned char i; |
179 | unsigned char i; |
180 | RequiredMotors = 0; |
180 | RequiredMotors = 0; |
181 | eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
181 | eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
182 | for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;} |
182 | for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;} |
183 | } |
183 | } |
184 | else // default |
184 | else // default |
185 | { |
185 | { |
186 | unsigned char i; |
186 | unsigned char i; |
187 | printf("\n\rGenerating default Mixer Table"); |
187 | printf("\n\rGenerating default Mixer Table"); |
188 | for(i=0; i<16;i++) { Mixer.Motor[i][0] = 0;Mixer.Motor[i][1] = 0;Mixer.Motor[i][2] = 0;Mixer.Motor[i][3] = 0;}; |
188 | for(i=0; i<16;i++) { Mixer.Motor[i][0] = 0;Mixer.Motor[i][1] = 0;Mixer.Motor[i][2] = 0;Mixer.Motor[i][3] = 0;}; |
189 | // default = Quadro |
189 | // default = Quadro |
190 | Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] = 0; Mixer.Motor[0][3] = +64; |
190 | Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] = 0; Mixer.Motor[0][3] = +64; |
191 | Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] = 0; Mixer.Motor[1][3] = +64; |
191 | Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] = 0; Mixer.Motor[1][3] = +64; |
192 | Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] = 0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64; |
192 | Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] = 0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64; |
193 | Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] = 0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64; |
193 | Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] = 0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64; |
194 | Mixer.Revision = MIXER_REVISION; |
194 | Mixer.Revision = MIXER_REVISION; |
195 | memcpy(Mixer.Name, "Quadro\0", 11); |
195 | memcpy(Mixer.Name, "Quadro\0", 11); |
196 | eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
196 | eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
197 | } |
197 | } |
198 | printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors); |
198 | printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors); |
Line 229... | Line 229... | ||
229 | for (unsigned char i=1;i<6;i++) |
229 | for (unsigned char i=1;i<6;i++) |
230 | { |
230 | { |
231 | if(i==2) DefaultKonstanten2(); // Kamera |
231 | if(i==2) DefaultKonstanten2(); // Kamera |
232 | if(i==3) DefaultKonstanten3(); // Beginner |
232 | if(i==3) DefaultKonstanten3(); // Beginner |
233 | if(i>3) DefaultKonstanten2(); // Kamera |
233 | if(i>3) DefaultKonstanten2(); // Kamera |
234 | if(PlatinenVersion >= 20) |
234 | if(PlatinenVersion >= 20) |
235 | { |
235 | { |
236 | EE_Parameter.Gyro_D = 5; |
236 | EE_Parameter.Gyro_D = 5; |
237 | EE_Parameter.Driftkomp = 0; |
237 | EE_Parameter.Driftkomp = 0; |
238 | EE_Parameter.GyroAccFaktor = 27; |
238 | EE_Parameter.GyroAccFaktor = 27; |
239 | EE_Parameter.WinkelUmschlagNick = 78; |
239 | EE_Parameter.WinkelUmschlagNick = 78; |
240 | EE_Parameter.WinkelUmschlagRoll = 78; |
240 | EE_Parameter.WinkelUmschlagRoll = 78; |
Line 295... | Line 295... | ||
295 | printf("\n\rBatt:"); |
295 | printf("\n\rBatt:"); |
296 | if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung |
296 | if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung |
297 | { |
297 | { |
298 | timer = SetDelay(500); |
298 | timer = SetDelay(500); |
299 | while (!CheckDelay(timer)); |
299 | while (!CheckDelay(timer)); |
300 | if(UBat < 130) |
300 | if(UBat < 130) |
301 | { |
301 | { |
302 | BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung; |
302 | BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung; |
303 | Piep(3,200); |
303 | Piep(3,200); |
304 | printf(" 3 Cells "); |
304 | printf(" 3 Cells "); |
305 | } |
305 | } |
306 | else |
306 | else |
307 | { |
307 | { |
308 | BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung; |
308 | BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung; |
309 | Piep(4,200); |
309 | Piep(4,200); |
310 | printf(" 4 Cells "); |
310 | printf(" 4 Cells "); |
311 | } |
311 | } |
Line 329... | Line 329... | ||
329 | ExternStickNick = 0; |
329 | ExternStickNick = 0; |
330 | ExternStickRoll = 0; |
330 | ExternStickRoll = 0; |
331 | ExternStickGier = 0; |
331 | ExternStickGier = 0; |
332 | } |
332 | } |
333 | if(SenderOkay) SenderOkay--; |
333 | if(SenderOkay) SenderOkay--; |
334 | else |
334 | else |
335 | { |
335 | { |
336 | TIMSK1 |= _BV(ICIE1); // enable PPM-Input |
336 | TIMSK1 |= _BV(ICIE1); // enable PPM-Input |
337 | } |
337 | } |
338 | //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160; |
338 | //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160; |
339 | //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101; |
339 | //if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101; |
340 | if(NaviDataOkay) |
340 | if(NaviDataOkay) |
341 | { |
341 | { |
342 | if(--NaviDataOkay == 0) |
342 | if(--NaviDataOkay == 0) |
343 | { |
343 | { |
344 | GPS_Nick = 0; |
344 | GPS_Nick = 0; |
345 | GPS_Roll = 0; |
345 | GPS_Roll = 0; |
346 | } |
346 | } |
347 | } |
347 | } |
348 | if(!--I2CTimeout || MissingMotor) |
348 | if(!--I2CTimeout || MissingMotor) |
349 | { |
349 | { |
350 | if(!I2CTimeout) |
350 | if(!I2CTimeout) |
351 | { |
351 | { |
352 | i2c_reset(); |
352 | i2c_reset(); |
353 | I2CTimeout = 5; |
353 | I2CTimeout = 5; |
354 | } |
354 | } |
355 | if((BeepMuster == 0xffff) && MotorenEin) |
355 | if((BeepMuster == 0xffff) && MotorenEin) |
Line 365... | Line 365... | ||
365 | if(SIO_DEBUG && (!UpdateMotor || !MotorenEin)) |
365 | if(SIO_DEBUG && (!UpdateMotor || !MotorenEin)) |
366 | { |
366 | { |
367 | DatenUebertragung(); |
367 | DatenUebertragung(); |
368 | BearbeiteRxDaten(); |
368 | BearbeiteRxDaten(); |
369 | } |
369 | } |
370 | else BearbeiteRxDaten(); |
370 | else BearbeiteRxDaten(); |
371 | if(CheckDelay(timer)) |
371 | if(CheckDelay(timer)) |
372 | { |
372 | { |
373 | if(UBat < BattLowVoltageWarning) |
373 | if(UBat < BattLowVoltageWarning) |
- | 374 | { |
|
374 | { |
375 | MikroKopterFlags |= FLAG_LOWBAT; |
375 | if(BeepMuster == 0xffff) |
376 | if(BeepMuster == 0xffff) |
376 | { |
377 | { |
377 | beeptime = 6000; |
378 | beeptime = 6000; |
378 | BeepMuster = 0x0300; |
379 | BeepMuster = 0x0300; |
379 | } |
380 | } |
380 | } |
381 | } |
- | 382 | else MikroKopterFlags &= ~FLAG_LOWBAT; |
|
381 | SPI_StartTransmitPacket(); |
383 | SPI_StartTransmitPacket(); |
382 | 384 | ||
383 | SendSPI = 4; |
385 | SendSPI = 4; |
384 | timer = SetDelay(20); |
386 | timer = SetDelay(20); |
385 | } |
387 | } |
386 | LED_Update(); |
388 | LED_Update(); |
387 | } |
389 | } |
388 | if(!SendSPI) { SPI_TransmitByte(); } |
390 | if(!SendSPI) { SPI_TransmitByte(); } |
389 | } |
391 | } |
390 | return (1); |
392 | return (1); |