WebSVN - FlightCtrl - Diff - Rev 1419 and 1420 - /branches/V0.78f_ACC-HH



         timer = SetDelay(500);
         if(print) while (!CheckDelay(timer));
         if(UBat < 130)
          {
           BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung;
           if(print)
            {
                 Piep(3,200);
             printf(" 3 Cells  ");
                }
          }
         else
          {
           BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung;
           if(print)
            {
                 Piep(4,200);
             printf(" 4 Cells  ");
                }
          }
    }
    else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung;
//      if(BattLowVoltageWarning < 93) BattLowVoltageWarning = 93;
   if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10);

// + Check connected BL-Ctrls
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        printf("\n\rFound BL-Ctrl: ");
    motorread = 0;   UpdateMotor = 0;   SendMotorData(); while(!UpdateMotor); motorread = 0;  // read the first I2C-Data
    timer = SetDelay(2000);
 
 
        for(i=0; i < MAX_MOTORS; i++)
         {
          UpdateMotor = 0;
      SendMotorData();
          while(!UpdateMotor);

                }
          if(MotorPresent[i]) printf("%d ",i+1);
     }
        for(i=0; i < MAX_MOTORS; i++)
         {
          if(!MotorPresent[i] && Mixer.Motor[i][0] > 0)
           {
            printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);
                ServoActive = 1; // just in case the FC would be used as camera-stabilizer
           }
          MotorError[i] = 0;
     }
        printf("\n\r===================================");
    SendMotorData();
//printf("\n size: %u",STRUCT_PARAM_LAENGE);

         EE_Parameter.Kanalbelegung[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]);
         EE_Parameter.Kanalbelegung[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]);
         EE_Parameter.Kanalbelegung[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]);
         EE_Parameter.Kanalbelegung[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]);
         EE_Parameter.Kanalbelegung[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]);
 
         if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]) < 255)
                  {
                   EE_Parameter.Kanalbelegung[K_POTI5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]);
           EE_Parameter.Kanalbelegung[K_POTI6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+9]);
           EE_Parameter.Kanalbelegung[K_POTI7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+10]);
           EE_Parameter.Kanalbelegung[K_POTI8] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+11]);
                  }
                  else
                  {
           EE_Parameter.Kanalbelegung[K_POTI5] = 9;
           EE_Parameter.Kanalbelegung[K_POTI6] = 10;
           EE_Parameter.Kanalbelegung[K_POTI7] = 11;
           EE_Parameter.Kanalbelegung[K_POTI8] = 12;

//SpektrumBinding();
    timer = SetDelay(2000);
        timerJeti = SetDelay(250);
        while (1)
        {
 
        if(CheckDelay(timerJeti))
        {
          timerJeti = SetDelay(100);
          JetiBoxPolling();
        }
 
        if(UpdateMotor && AdReady)      // ReglerIntervall
            {
                    UpdateMotor=0;
            if(WinkelOut.CalcState) CalMk3Mag();
            else MotorRegler();
            SendMotorData();
            ROT_OFF;
            if(SenderOkay)  SenderOkay--;
                        else TIMSK1 |= _BV(ICIE1); // enable PPM-Input
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160;
//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
 
 
 
 
 
 
 
            if(!--I2CTimeout || MissingMotor)
                {
                  if(!I2CTimeout)
                                   {
                                    i2c_reset();

                }
            else
                {
                 ROT_OFF;
                }
 
            if(SIO_DEBUG && (!UpdateMotor || !MotorenEin))
              {
               DatenUebertragung();
               BearbeiteRxDaten();
              }
                        else BearbeiteRxDaten();
 
 
                        if(CheckDelay(timer))
                        {
 
                                timer += 20; // 20 ms interval
 
 
 
                                if(PcZugriff) PcZugriff--;
                                else
                                {
                                        ExternControl.Config = 0;
                                        ExternStickNick = 0;
                                        ExternStickRoll = 0;
                                        ExternStickGier = 0;
                                        if(BeepMuster == 0xffff && SenderOkay == 0)
                                        {
                                                beeptime = 15000;
                                                BeepMuster = 0x0c00;
                                        }
                                }
                                if(NaviDataOkay)
                                {
                                        NaviDataOkay--;
                                        FCFlags &= ~FCFLAG_SPI_RX_ERR;
                                }
                                else
                                {
                                        GPS_Nick = 0;
                                        GPS_Roll = 0;
                                        FCFlags |= FCFLAG_SPI_RX_ERR;
                                }
                           if(UBat < BattLowVoltageWarning)
                                {
                                        FCFlags |= FCFLAG_LOWBAT;
                                        if(BeepMuster == 0xffff)
                                        {
                                                beeptime = 6000;
                                                BeepMuster = 0x0300;
                                        }
                                }
                                else FCFlags &= ~FCFLAG_LOWBAT;
 
                                SPI_StartTransmitPacket();
                                SendSPI = 4;
 
                                if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden
 
                                if(++timer2 == 2930)  // eine Minute
                                 {
                                   timer2 = 0;
                   FlugMinuten++;
                       FlugMinutenGesamt++;
                   eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2],FlugMinuten / 256);
                   eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1],FlugMinuten % 256);
                   eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES],FlugMinutenGesamt / 256);
                   eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES+1],FlugMinutenGesamt % 256);
                                   timer = SetDelay(20); // falls "timer += 20;" mal nicht geht
                             }
                        }
           LED_Update();
          }

Rev 1419	Rev 1420
Line 126...	Line 126...
126	timer = SetDelay(500);	126	timer = SetDelay(500);
127	if(print) while (!CheckDelay(timer));	127	if(print) while (!CheckDelay(timer));
128	if(UBat < 130)	128	if(UBat < 130)
129	{	129	{
130	BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung;	130	BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung;
131	if(print)	131	if(print)
132	{	132	{
133	Piep(3,200);	133	Piep(3,200);
134	printf(" 3 Cells ");	134	printf(" 3 Cells ");
135	}	135	}
136	}	136	}
137	else	137	else
138	{	138	{
139	BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung;	139	BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung;
140	if(print)	140	if(print)
141	{	141	{
142	Piep(4,200);	142	Piep(4,200);
143	printf(" 4 Cells ");	143	printf(" 4 Cells ");
144	}	144	}
145	}	145	}
146	}	146	}
147	else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung;	147	else BattLowVoltageWarning = EE_Parameter.UnterspannungsWarnung;
148	// if(BattLowVoltageWarning < 93) BattLowVoltageWarning = 93;	148	// if(BattLowVoltageWarning < 93) BattLowVoltageWarning = 93;
149	if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10);	149	if(print) printf(" Low warning level: %d.%d",BattLowVoltageWarning/10,BattLowVoltageWarning%10);
Line 236...	Line 236...
236	// + Check connected BL-Ctrls	236	// + Check connected BL-Ctrls
237	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	237	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
238	printf("\n\rFound BL-Ctrl: ");	238	printf("\n\rFound BL-Ctrl: ");
239	motorread = 0; UpdateMotor = 0; SendMotorData(); while(!UpdateMotor); motorread = 0; // read the first I2C-Data	239	motorread = 0; UpdateMotor = 0; SendMotorData(); while(!UpdateMotor); motorread = 0; // read the first I2C-Data
240	timer = SetDelay(2000);	240	timer = SetDelay(2000);
241		241
242		242
243	for(i=0; i < MAX_MOTORS; i++)	243	for(i=0; i < MAX_MOTORS; i++)
244	{	244	{
245	UpdateMotor = 0;	245	UpdateMotor = 0;
246	SendMotorData();	246	SendMotorData();
247	while(!UpdateMotor);	247	while(!UpdateMotor);
Line 251...	Line 251...
251	}	251	}
252	if(MotorPresent[i]) printf("%d ",i+1);	252	if(MotorPresent[i]) printf("%d ",i+1);
253	}	253	}
254	for(i=0; i < MAX_MOTORS; i++)	254	for(i=0; i < MAX_MOTORS; i++)
255	{	255	{
256	if(!MotorPresent[i] && Mixer.Motor[i][0] > 0)	256	if(!MotorPresent[i] && Mixer.Motor[i][0] > 0)
257	{	257	{
258	printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);	258	printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);
259	ServoActive = 1; // just in case the FC would be used as camera-stabilizer	259	ServoActive = 1; // just in case the FC would be used as camera-stabilizer
260	}	260	}
261	MotorError[i] = 0;	261	MotorError[i] = 0;
262	}	262	}
263	printf("\n\r===================================");	263	printf("\n\r===================================");
264	SendMotorData();	264	SendMotorData();
265	//printf("\n size: %u",STRUCT_PARAM_LAENGE);	265	//printf("\n size: %u",STRUCT_PARAM_LAENGE);
Line 293...	Line 293...
293	EE_Parameter.Kanalbelegung[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]);	293	EE_Parameter.Kanalbelegung[3] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+3]);
294	EE_Parameter.Kanalbelegung[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]);	294	EE_Parameter.Kanalbelegung[4] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+4]);
295	EE_Parameter.Kanalbelegung[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]);	295	EE_Parameter.Kanalbelegung[5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+5]);
296	EE_Parameter.Kanalbelegung[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]);	296	EE_Parameter.Kanalbelegung[6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+6]);
297	EE_Parameter.Kanalbelegung[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]);	297	EE_Parameter.Kanalbelegung[7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+7]);
298		298
299	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]) < 255)	299	if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]) < 255)
300	{	300	{
301	EE_Parameter.Kanalbelegung[K_POTI5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]);	301	EE_Parameter.Kanalbelegung[K_POTI5] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+8]);
302	EE_Parameter.Kanalbelegung[K_POTI6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+9]);	302	EE_Parameter.Kanalbelegung[K_POTI6] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+9]);
303	EE_Parameter.Kanalbelegung[K_POTI7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+10]);	303	EE_Parameter.Kanalbelegung[K_POTI7] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+10]);
304	EE_Parameter.Kanalbelegung[K_POTI8] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+11]);	304	EE_Parameter.Kanalbelegung[K_POTI8] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS+11]);
305	}	305	}
306	else	306	else
307	{	307	{
308	EE_Parameter.Kanalbelegung[K_POTI5] = 9;	308	EE_Parameter.Kanalbelegung[K_POTI5] = 9;
309	EE_Parameter.Kanalbelegung[K_POTI6] = 10;	309	EE_Parameter.Kanalbelegung[K_POTI6] = 10;
310	EE_Parameter.Kanalbelegung[K_POTI7] = 11;	310	EE_Parameter.Kanalbelegung[K_POTI7] = 11;
311	EE_Parameter.Kanalbelegung[K_POTI8] = 12;	311	EE_Parameter.Kanalbelegung[K_POTI8] = 12;
Line 365...	Line 365...
365	//SpektrumBinding();	365	//SpektrumBinding();
366	timer = SetDelay(2000);	366	timer = SetDelay(2000);
367	timerJeti = SetDelay(250);	367	timerJeti = SetDelay(250);
368	while (1)	368	while (1)
369	{	369	{
370		370
371	if(CheckDelay(timerJeti))	371	if(CheckDelay(timerJeti))
372	{	372	{
373	timerJeti = SetDelay(100);	373	timerJeti = SetDelay(100);
374	JetiBoxPolling();	374	JetiBoxPolling();
375	}	375	}
376		376
377	if(UpdateMotor && AdReady) // ReglerIntervall	377	if(UpdateMotor && AdReady) // ReglerIntervall
378	{	378	{
379	UpdateMotor=0;	379	UpdateMotor=0;
380	if(WinkelOut.CalcState) CalMk3Mag();	380	if(WinkelOut.CalcState) CalMk3Mag();
381	else MotorRegler();	381	else MotorRegler();
382	SendMotorData();	382	SendMotorData();
383	ROT_OFF;	383	ROT_OFF;
384	if(SenderOkay) SenderOkay--;	384	if(SenderOkay) SenderOkay--;
385	else TIMSK1 \|= _BV(ICIE1); // enable PPM-Input	385	else TIMSK1 \|= _BV(ICIE1); // enable PPM-Input
386	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	386	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
387	//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160;	387	//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 160 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 220) SenderOkay = 160;
388	//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101;	388	//if(HoehenReglerAktiv && NaviDataOkay && SenderOkay < 101 && SenderOkay > 10 && FromNaviCtrl_Value.SerialDataOkay > 1) SenderOkay = 101;
389	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	389	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
390	if(NaviDataOkay)	-
391	{	-
392	if(--NaviDataOkay == 0)	-
393	{	-
394	GPS_Nick = 0;	-
395	GPS_Roll = 0;	-
396	}	-
397	}	-
398		390
399	if(!--I2CTimeout \|\| MissingMotor)	391	if(!--I2CTimeout \|\| MissingMotor)
400	{	392	{
401	if(!I2CTimeout)	393	if(!I2CTimeout)
402	{	394	{
403	i2c_reset();	395	i2c_reset();
Line 412...	Line 404...
412	}	404	}
413	else	405	else
414	{	406	{
415	ROT_OFF;	407	ROT_OFF;
416	}	408	}
417		409
418	if(SIO_DEBUG && (!UpdateMotor \|\| !MotorenEin))	410	if(SIO_DEBUG && (!UpdateMotor \|\| !MotorenEin))
419	{	411	{
420	DatenUebertragung();	412	DatenUebertragung();
421	BearbeiteRxDaten();	413	BearbeiteRxDaten();
422	}	414	}
423	else BearbeiteRxDaten();	415	else BearbeiteRxDaten();
424		416
425		417
426	if(CheckDelay(timer))	418	if(CheckDelay(timer))
427	{	419	{
428		420
429	timer += 20;	421	timer += 20; // 20 ms interval
430		-
431		-
432		422
433	if(PcZugriff) PcZugriff--;	423	if(PcZugriff) PcZugriff--;
434	else	424	else
435	{	425	{
436	ExternControl.Config = 0;	426	ExternControl.Config = 0;
437	ExternStickNick = 0;	427	ExternStickNick = 0;
438	ExternStickRoll = 0;	428	ExternStickRoll = 0;
439	ExternStickGier = 0;	429	ExternStickGier = 0;
440	if(BeepMuster == 0xffff && SenderOkay == 0)	430	if(BeepMuster == 0xffff && SenderOkay == 0)
441	{	431	{
442	beeptime = 15000;	432	beeptime = 15000;
443	BeepMuster = 0x0c00;	433	BeepMuster = 0x0c00;
-		434	}
-		435	}
-		436	if(NaviDataOkay)
-		437	{
-		438	NaviDataOkay--;
-		439	FCFlags &= ~FCFLAG_SPI_RX_ERR;
-		440	}
-		441	else
-		442	{
444	}	443	GPS_Nick = 0;
445	}	444	GPS_Roll = 0;
-		445	FCFlags \|= FCFLAG_SPI_RX_ERR;
-		446	}
446	if(UBat < BattLowVoltageWarning)	447	if(UBat < BattLowVoltageWarning)
447	{	448	{
448	MikroKopterFlags \|= FLAG_LOWBAT;	449	FCFlags \|= FCFLAG_LOWBAT;
449	if(BeepMuster == 0xffff)	450	if(BeepMuster == 0xffff)
450	{	451	{
451	beeptime = 6000;	452	beeptime = 6000;
452	BeepMuster = 0x0300;	453	BeepMuster = 0x0300;
453	}	454	}
454	}	455	}
455	else MikroKopterFlags &= ~FLAG_LOWBAT;	456	else FCFlags &= ~FCFLAG_LOWBAT;
456		457
457	SPI_StartTransmitPacket();	458	SPI_StartTransmitPacket();
458	SendSPI = 4;	459	SendSPI = 4;
459		460
460	if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden	461	if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden
461		462
462	if(++timer2 == 2930) // eine Minute	463	if(++timer2 == 2930) // eine Minute
463	{	464	{
464	timer2 = 0;	465	timer2 = 0;
465	FlugMinuten++;	466	FlugMinuten++;
466	FlugMinutenGesamt++;	467	FlugMinutenGesamt++;
467	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2],FlugMinuten / 256);	468	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2],FlugMinuten / 256);
468	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1],FlugMinuten % 256);	469	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES2+1],FlugMinuten % 256);
469	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES],FlugMinutenGesamt / 256);	470	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES],FlugMinutenGesamt / 256);
470	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES+1],FlugMinutenGesamt % 256);	471	eeprom_write_byte(&EEPromArray[EEPROM_ADR_MINUTES+1],FlugMinutenGesamt % 256);
471	timer = SetDelay(20); // falls "timer += 20;" mal nicht geht	472	timer = SetDelay(20); // falls "timer += 20;" mal nicht geht
472	}	473	}
473	}	474	}
474	LED_Update();	475	LED_Update();
475	}	476	}

Subversion Repositories FlightCtrl

(root)/branches/V0.78f_ACC-HH_MartinR/main.c @ 1905 - Rev 1419 → 1420