WebSVN - FlightCtrl - Diff - Rev 1232 and 1254 - /branches/V0.88n_ACC-HH_HR



        sei();

 
        printf("\n\r===================================");
        printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion/10,PlatinenVersion%10, VERSION_MAJOR, VERSION_MINOR,VERSION_PATCH + 'a');
    if(UCSR1A == 0x20 && UCSR1C == 0x06)  // initial Values for 644P
     {
      Uart1Init();
     }
        GRN_ON;

        ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 9); // read only the first bytes
 
    if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && // Check Revision in the first Byte
       (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff))                   // Settings reset via Koptertool
        {
     unsigned char i;
         RequiredMotors = 0;
     eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
     for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;}
        }
        else // default
        {
     unsigned char i;
         printf("\n\rGenerating default Mixer Table");
         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;};
     // default = Quadro
     Mixer.Motor[0][0] = 64; Mixer.Motor[0][1] = +64; Mixer.Motor[0][2] =   0; Mixer.Motor[0][3] = +64;
     Mixer.Motor[1][0] = 64; Mixer.Motor[1][1] = -64; Mixer.Motor[1][2] =   0; Mixer.Motor[1][3] = +64;
     Mixer.Motor[2][0] = 64; Mixer.Motor[2][1] =   0; Mixer.Motor[2][2] = -64; Mixer.Motor[2][3] = -64;
     Mixer.Motor[3][0] = 64; Mixer.Motor[3][1] =   0; Mixer.Motor[3][2] = +64; Mixer.Motor[3][3] = -64;
         Mixer.Revision = MIXER_REVISION;
     memcpy(Mixer.Name, "Quadro\0", 11);
     eeprom_write_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
    }
    printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name,RequiredMotors);

          for (unsigned char i=1;i<6;i++)
      {
       if(i==2) DefaultKonstanten2(); // Kamera
       if(i==3) DefaultKonstanten3(); // Beginner
       if(i>3)  DefaultKonstanten2(); // Kamera
           if(PlatinenVersion >= 20)
            {
                 EE_Parameter.Gyro_D = 5;
                 EE_Parameter.Driftkomp = 0;
                 EE_Parameter.GyroAccFaktor = 27;
         EE_Parameter.WinkelUmschlagNick = 78;
         EE_Parameter.WinkelUmschlagRoll = 78;

   printf("\n\rBatt:");
   if(EE_Parameter.UnterspannungsWarnung < 50) // automatische Zellenerkennung
    {
         timer = SetDelay(500);
         while (!CheckDelay(timer));
         if(UBat < 130)
          {
           BattLowVoltageWarning = 3 * EE_Parameter.UnterspannungsWarnung;
           Piep(3,200);
           printf(" 3 Cells  ");
          }
         else
          {
           BattLowVoltageWarning = 4 * EE_Parameter.UnterspannungsWarnung;
           Piep(4,200);
           printf(" 4 Cells  ");
          }

               ExternStickNick = 0;
               ExternStickRoll = 0;
               ExternStickGier = 0;
              }
            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(NaviDataOkay)
                         {
                          if(--NaviDataOkay == 0)
                           {
                    GPS_Nick = 0;
                GPS_Roll = 0;
               }
              }
            if(!--I2CTimeout || MissingMotor)
                {
                  if(!I2CTimeout)
                                   {
                                    i2c_reset();
                    I2CTimeout = 5;
                                        }
                  if((BeepMuster == 0xffff) && MotorenEin)

            if(SIO_DEBUG && (!UpdateMotor || !MotorenEin))
              {
               DatenUebertragung();
               BearbeiteRxDaten();
              }
                        else BearbeiteRxDaten();
                        if(CheckDelay(timer))
                        {
                                if(UBat < BattLowVoltageWarning)
                                {
                                        MikroKopterFlags |= FLAG_LOWBAT;
                                        if(BeepMuster == 0xffff)
                                        {
                                                beeptime = 6000;
                                                BeepMuster = 0x0300;
                                        }
                                }
                                else MikroKopterFlags &= ~FLAG_LOWBAT;
                                SPI_StartTransmitPacket();
 
                                SendSPI = 4;
                                timer = SetDelay(20);
                        }
           LED_Update();
          }
     if(!SendSPI) { SPI_TransmitByte(); }
    }
 return (1);

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);

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/main.c @ 2177 - Rev 1232 → 1254