main.c

四轴飞行器代码.rar

/*#######################################################################################
Flight Control
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Regler f黵 Brushless-Motoren
// + ATMEGA8 mit 8MHz
// + Nur f黵 den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + Copyright (c) 12.2007 Holger Buss
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt f黵 das gesamte Projekt (Hardware, Software, Bin鋜files, Sourcecode und Dokumentation), 
// + dass eine Nutzung (auch auszugsweise) nur f黵 den privaten (nicht-kommerziellen) Gebrauch zul鋝sig ist. 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns ([e-mail]) Kontakt 
// + bzgl. der Nutzungsbedingungen aufzunehmen. 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best點kung und Verkauf von Platinen oder Baus鋞zen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver鰂fentlicht, 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m黶sen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien ver鰂fentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gew鋒r auf Fehlerfreiheit, Vollst鋘digkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir 黚ernehmen keinerlei Haftung f黵 direkte oder indirekte Personen- oder Sachsch鋎en
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur 
// + mit unserer Zustimmung zul鋝sig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, 
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet 
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted 
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be 
// +     clearly linked as origin 
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE. 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "main.h"

unsigned int  PWM = 0; 
unsigned int  Strom = 0,RuheStrom; //ca. in 0,1A
unsigned char Strom_max = 0;
unsigned char Mittelstrom = 0; 
unsigned int  Drehzahl = 0;  // in 100UPM  60 = 6000
unsigned int  KommutierDelay = 10;
unsigned int  I2C_Timeout = 0;
unsigned int SIO_Timeout = 0;
unsigned int  SollDrehzahl = 0;
unsigned int  IstDrehzahl = 0;
unsigned int  DrehZahlTabelle[256];//vorberechnete Werte zur Drehzahlerfassung
unsigned char ZeitFuerBerechnungen = 1;
unsigned char MotorAnwerfen = 0;
unsigned char MotorGestoppt = 1;
unsigned char MaxPWM = MAX_PWM;
unsigned int  CntKommutierungen = 0;
unsigned int  SIO_Drehzahl = 0;
unsigned char ZeitZumAdWandeln = 1;
unsigned char MotorAdresse = 1;
unsigned char PPM_Betrieb = 1;
unsigned char HwVersion;
unsigned char IntRef = 0;
unsigned int MinUpmPulse;
//############################################################################
//
void SetPWM(void)
//############################################################################
{
    unsigned char tmp_pwm; 
    tmp_pwm = PWM;
    if(tmp_pwm > MaxPWM)    // Strombegrenzung
        {
        tmp_pwm = MaxPWM;
        PORTC |= ROT; 
        } 
    if(Strom > MAX_STROM)   // Strombegrenzung
        {
        OCR1A = 0; OCR1B = 0; OCR2 = 0;
        PORTD &= ~0x38;
        PORTC |= ROT; 
        DebugOut.Analog[6]++;
        Strom--;
        }
    else 
        {
        #ifdef  _32KHZ 
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
        #endif 

        #ifdef  _16KHZ 
        //OCR1A = 2 * (int)tmp_pwm; OCR1B = 2 * (int)tmp_pwm; OCR2  = tmp_pwm;
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
        #endif 
        }
//GRN_ON;
}

void DebugAusgaben(void)
{
    DebugOut.Analog[0] = Strom;
    DebugOut.Analog[1] = Mittelstrom;
    DebugOut.Analog[2] = SIO_Drehzahl;
    DebugOut.Analog[3] = PPM_Signal;
    DebugOut.Analog[4] = OCR2;
//    DebugOut.Analog[5] = PWM;
}

//############################################################################
//
void PWM_Init(void)
//############################################################################
{
    PWM_OFF;
    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (0 << WGM12) | 
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
/*    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (1 << WGM12) | 
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
*/
}

//############################################################################
//
void Wait(unsigned char dauer)
//############################################################################
{
    dauer = (unsigned char)TCNT0 + dauer;
    while((TCNT0 - dauer) & 0x80);
}

void RotBlink(unsigned char anz)
{
sei(); // Interrupts ein
 while(anz--)
  {
   PORTC |= ROT;
   Delay_ms(300);    
   PORTC &= ~ROT;
   Delay_ms(300);    
  }
   Delay_ms(1000);    
}

//############################################################################
//
char Anwerfen(unsigned char pwm)
//############################################################################
{
    unsigned long timer = 300,i;
    DISABLE_SENSE_INT;
    PWM = 5;
    SetPWM();
    Manuell();
//    Delay_ms(200);
                    MinUpmPulse = SetDelay(300);
                    while(!CheckDelay(MinUpmPulse)) 
                    {
                     FastADConvert();
                      if(Strom > 120) 
                      {
                        STEUER_OFF; // Abschalten wegen Kurzschluss
                        RotBlink(10);
                        return(0);
                      }  
                    }
    PWM = pwm;
    while(1)
        {
        for(i=0;i<timer; i++) 
            {
            if(!UebertragungAbgeschlossen)  SendUart();
            else DatenUebertragung();
            Wait(100);  // warten
            } 
        DebugAusgaben();
        FastADConvert();
        if(Strom > 60) 
          {
            STEUER_OFF; // Abschalten wegen Kurzschluss
            RotBlink(10);
            return(0);
          }  
         
        timer-= timer/15+1;
        if(timer < 25) { if(TEST_MANUELL) timer = 25; else return(1); }
        Manuell();
        Phase++;
        Phase %= 6;
        AdConvert();
        PWM = pwm;
        SetPWM();
        if(SENSE) 
            {
            PORTD ^= GRUEN;
            } 
        }
}

/*
#define SENSE_A ADMUX = 0;
#define SENSE_B ADMUX = 1;
#define SENSE_C ADMUX = 2;

#define ClrSENSE            ACSR |= 0x10
#define SENSE               ((ACSR & 0x10))
#define SENSE_L             (!(ACSR & 0x20))
#define SENSE_H             ((ACSR & 0x20))
*/


#define TEST_STROMGRENZE 120
unsigned char DelayM(unsigned int timer)
{
 while(timer--)
  {
   FastADConvert();
   if(Strom > (TEST_STROMGRENZE + RuheStrom))
       {
        FETS_OFF;
        return(1);
       } 
  }
 return(0);  
}

unsigned char Delay(unsigned int timer)
{
 while(timer--)
  {
//   if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
  }
 return(0);  
}

/*
void ShowSense(void)
{
 if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}

}
*/
#define HIGH_A_EIN PORTB |= 0x08
#define HIGH_B_EIN PORTB |= 0x04
#define HIGH_C_EIN PORTB |= 0x02
#define LOW_A_EIN  PORTD |= 0x08
#define LOW_B_EIN  PORTD |= 0x10
#define LOW_C_EIN  PORTD |= 0x20

void MotorTon(void)
//############################################################################
{
    unsigned char ADR_TAB[9] = {0,0,2,1,3,4,5,6,7};
    unsigned int timer = 300,i;
    unsigned int t = 0;
    unsigned char anz = 0,MosfetOkay = 0, grenze = 50;

    PORTC &= ~ROT;
    Delay_ms(300 * ADR_TAB[MotorAdresse]);    
    DISABLE_SENSE_INT;
    cli();//Globale Interrupts Ausschalten
    uart_putchar('\n');
    STEUER_OFF;
    Strom_max = 0;
    DelayM(50);
    RuheStrom = Strom_max;
//    uart_putchar(RuheStrom + 'A');
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ High-Mosfets auf Kurzschluss testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Strom = 0;
/*
    LOW_B_EIN; 
    HIGH_A_EIN; 
    if(DelayM(3))
       {
        anz = 1;
        uart_putchar('1');
       }
    FETS_OFF;
    Delay(1000);
    Strom = 0;
    LOW_A_EIN;
    HIGH_B_EIN; 
    if(DelayM(3))
       {
        anz = 2;
        uart_putchar('2');
       }
    FETS_OFF;
    Delay(1000);
    Strom = 0;
    LOW_B_EIN; // Low C ein
    HIGH_C_EIN; // High B ein
    if(DelayM(3))
       {
        anz = 3;
        uart_putchar('3');
       }
    FETS_OFF;
    Delay(1000);
    LOW_A_EIN; // Low  A ein; und A gegen C
    HIGH_C_EIN; // High C ein
    if(DelayM(3))
       {
        anz = 3;
        uart_putchar('7');
       }
    FETS_OFF;
    DelayM(10000);

if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten
*/
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ LOW-Mosfets auf Schalten und Kurzschluss testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 if(UDR == ' ') {t = 65535; grenze = 40; uart_putchar('_');} else t = 1000; // Ausf黨rlicher Test
 Strom = 0;
 for(i=0;i<t;i++)
 {
  LOW_A_EIN; 
  DelayM(1);
  FETS_OFF;
  Delay(5);
  HIGH_A_EIN;
  DelayM(1);
  FETS_OFF;
  if(Strom > grenze + RuheStrom) {anz = 4; uart_putchar('4'); FETS_OFF; break;}
  Delay(5);
 }
 Delay(10000);

 Strom = 0;
 for(i=0;i<t;i++)
 {
  LOW_B_EIN; 
  DelayM(1);
  FETS_OFF;
  Delay(5);
  HIGH_B_EIN;
  DelayM(1);
  FETS_OFF;
  if(Strom > grenze + RuheStrom) {anz = 5; uart_putchar('5'); FETS_OFF;break;}
  Delay(5);
 } 

 Strom = 0;
 Delay(10000);

 for(i=0;i<t;i++)
 {
  LOW_C_EIN; 
  DelayM(1);
  FETS_OFF;
  Delay(5);
  HIGH_C_EIN;
  DelayM(1);
  FETS_OFF;
  if(Strom > grenze + RuheStrom) {anz = 6; uart_putchar('6'); FETS_OFF; break;}
  Delay(5);
 } 

 if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ High-Mosfets auf Schalten testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    SENSE_A;
    FETS_OFF;
    LOW_B_EIN; // Low B ein
    LOW_C_EIN; // Low C ein
    Strom = 0;