WebSVN - FlightCtrl - Diff - Rev 1378 and 1550 - /branches/V0.78g

 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 unsigned char EEPromArray[E2END+1] EEMEM;
 unsigned char PlatinenVersion = 10;
 unsigned char SendVersionToNavi = 1;
 unsigned char BattLowVoltageWarning = 94;
+unsigned int FlugMinuten = 0,FlugMinutenGesamt = 0;
+unsigned int FlugSekunden = 0;
+pVoidFnct_pVoidFnctChar_const_fmt _printf_P;
 unsigned int FlugMinuten = 0,FlugMinutenGesamt = 0;
 // -- Parametersatz aus EEPROM lesen ---
 // number [1..5]
 void ReadParameterSet(unsigned char number, unsigned char *buffer, unsigned char length)
+{
+{
    if(number > 5) number = 5;
    if(number < 1) return;
    eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_PARAM_BEGIN + length * (number - 1)], length);
    eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_LENGTH], length); // Länge der Datensätze merken
-   eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_CHANNELS], 8); // 8 Kanäle merken
+   eeprom_write_block(buffer, &EEPromArray[EEPROM_ADR_CHANNELS], 12); // 12 Kanäle merken
    SetActiveParamSetNumber(number);
    LED_Init();
+}
                 SetActiveParamSetNumber(set);                           // diesen Parametersatz als aktuell merken
+        }
         return(set);
+}
 void SetActiveParamSetNumber(unsigned char number)
+{
         if(number > 5) number = 5;
         if(number < 1) return;
    else Piep(WinkelOut.CalcState,150);
+  }
   DebugOut.Analog[19] = WinkelOut.CalcState;
+}
 void LipoDetection(unsigned char print)
+{
    unsigned int timer;
    if(print) printf("\n\rBatt:");
          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);
 //############################################################################
 //Hauptprogramm
 int main (void)
 //############################################################################
+{
-        unsigned int timer,i,timer2 = 0;
+        unsigned int timer,i,timer2 = 0, timerPolling;
+        unsigned char RequiredMotors = 0;
     DDRB  = 0x00;
     PORTB = 0x00;
     for(timer = 0; timer < 1000; timer++); // verzögern
     if(PINB & 0x01)
+     {
         UART_Init();
     rc_sum_init();
         ADC_Init();
         i2c_init();
         SPI_MasterInit();
+        Capacity_Init();
+        LIBFC_Init();
+        GRN_ON;
+    sei();
-        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');
-        printf("\n\rthe use of this software is only permitted \n\ron original MikroKopter-Hardware");
-        printf("\n\rwww.MikroKopter.de (c) HiSystems GmbH");
-        printf("\n\r===================================");
-    if(UCSR1A == 0x20 && UCSR1C == 0x06)  // initial Values for 644P
-     {
-      Uart1Init();
-     }
-        GRN_ON;
+        ReadParameterSet(3, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 13); // read only the first bytes
         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
        (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff))                   // Settings reset via Koptertool
      unsigned char i;
-         RequiredMotors = 0;
+     unsigned char i;
      eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
      for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;}
          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);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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(Mixer.Motor[i][0] > 0) // wait max 2 sec for the BL-Ctrls to wake up
+        {
-             while(!CheckDelay(timer) && !MotorPresent[i]) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);};
+             while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);};
+                }
-          if(MotorPresent[i]) printf("%d ",i+1);
+          if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) printf("%d ",i+1);
+     }
         for(i=0; i < MAX_MOTORS; i++)
+         {
-          if(!MotorPresent[i] && Mixer.Motor[i][0] > 0)
+          if(!(Motor[i].State & MOTOR_STATE_PRESENT_MASK) && 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;
+          Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter
+     }
         printf("\n\r===================================");
     SendMotorData();
+//printf("\n size: %u",STRUCT_PARAM_LAENGE);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Check Settings
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
          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;
+                  }
          if(i==1) printf(": Generating Default-Parameter using old Stick Settings");
                 } else DefaultStickMapping();
        WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
+      }
           SetActiveParamSetNumber(3); // default-Setting
         ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
     printf("\n\rUsing parameterset %d", GetActiveParamSetNumber());
-        if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
+        //if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
+         {
            printf("\n\rCalibrating pressure sensor..");
     LcdClear();
     I2CTimeout = 5000;
     WinkelOut.Orientation = 1;
+    LipoDetection(1);
+        LIBFC_ReceiverInit();
     LipoDetection(1);
         printf("\n\r===================================\n\r");
 //SpektrumBinding();
+    timer = SetDelay(2000);
+        timerPolling = SetDelay(250);
     timer = SetDelay(2000);
         while (1)
+        {
+        if(CheckDelay(timerPolling))
+        {
+          timerPolling = SetDelay(100);
+          LIBFC_Polling();
+        }
-        {
-            if(UpdateMotor && AdReady)      // ReglerIntervall
+        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(NaviDataOkay)
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                         {
-                          if(--NaviDataOkay == 0)
-                           {
-                    GPS_Nick = 0;
-                GPS_Roll = 0;
-               }
             if(!--I2CTimeout || MissingMotor)
+                {
                   if(!I2CTimeout)
+                                   {
                                     i2c_reset();
                     I2CTimeout = 5;
+                                        DebugOut.Analog[28]++; // I2C-Error
-                                        DebugOut.Analog[28]++; // I2C-Error
+                                        FCFlags |= FCFLAG_I2CERR;
+                                   }
                   if((BeepMuster == 0xffff) && MotorenEin)
+                   {
                     beeptime = 10000;
                     BeepMuster = 0x0080;
+                   }
+                }
             else
+                {
+                 ROT_OFF;
-                 ROT_OFF;
+                                 if(!beeptime) FCFlags &= ~FCFLAG_I2CERR;
+                }
             if(SIO_DEBUG && (!UpdateMotor || !MotorenEin))
+              {
                DatenUebertragung();
                BearbeiteRxDaten();
+              }
                         else BearbeiteRxDaten();
                         if(CheckDelay(timer))
+                        {
-                        {
+                                static unsigned char second;
-                          timer += 20;
+                                timer += 20; // 20 ms interval
-              if(PcZugriff) PcZugriff--;
+                                if(PcZugriff) PcZugriff--;
-              else
+                                else
-               {
+                                {
-                            ExternControl.Config = 0;
+                                        ExternControl.Config = 0;
-                ExternStickNick = 0;
+                                        ExternStickNick = 0;
-                ExternStickRoll = 0;
+                                        ExternStickRoll = 0;
-                ExternStickGier = 0;
+                                        ExternStickGier = 0;
-                if(BeepMuster == 0xffff && SenderOkay == 0)
+                                        if(BeepMuster == 0xffff && SenderOkay == 0)
-                {
+                                        {
-                 beeptime = 15000;
+                                                beeptime = 15000;
+                                                BeepMuster = 0x0c00;
+                                        }
+                                }
+                                if(NaviDataOkay)
+                                {
+                                        NaviDataOkay--;
+                                        FCFlags &= ~FCFLAG_SPI_RX_ERR;
+                                }
+                                else
-                 BeepMuster = 0x0c00;
+                                {
-                }
+                                        GPS_Nick = 0;
+                                        GPS_Roll = 0;
+                                        if(!beeptime) FCFlags |= FCFLAG_SPI_RX_ERR;
+                                }
                            if(UBat < BattLowVoltageWarning)
                                 {
-                                        MikroKopterFlags |= FLAG_LOWBAT;
+                                        FCFlags |= FCFLAG_LOWBAT;
                                         if(BeepMuster == 0xffff)
+                                        {
                                                 beeptime = 6000;
                                                 BeepMuster = 0x0300;
+                                        }
                                 }
+                                else if(!beeptime) FCFlags &= ~FCFLAG_LOWBAT;
                                 else MikroKopterFlags &= ~FLAG_LOWBAT;
                                 SPI_StartTransmitPacket();
                                 SendSPI = 4;
+                                if(!MotorenEin) timer2 = 1450; // 0,5 Minuten aufrunden
+                                else
+                if(++second == 49)
+                                 {
+                                   second = 0;
+                                   FlugSekunden++;
+                                 }
                                 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();
-           LED_Update();
+           Capacity_Update();
+          }
      if(!SendSPI) { SPI_TransmitByte(); }
+    }
  return (1);

Subversion Repositories FlightCtrl

(root)/branches/V0.78g_WKN-hef/main.c @ 1573 - Rev 1378 → 1550