WebSVN - FlightCtrl - Diff - Rev 1227 and 1342 - /branches/V0.74d_CRD_killagreg_Batteriekappazität


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than 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 <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
 
#include "eeprom.h"
#include "main.h"
#include "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "fc.h"
#include "rc.h"
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
#include "ubx.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
 
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE   0
#define TRUE    1
 
//int8_t test __attribute__ ((section (".noinit")));
uint8_t Request_VerInfo                 = FALSE;
uint8_t Request_ExternalControl = FALSE;
uint8_t Request_Display                 = FALSE;
uint8_t Request_Display1                = FALSE;
uint8_t Request_DebugData               = FALSE;
uint8_t Request_Data3D                  = FALSE;
uint8_t Request_DebugLabel              = 255;
uint8_t Request_PPMChannels     = FALSE;
uint8_t Request_MotorTest               = FALSE;
uint8_t DisplayLine = 0;
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
volatile uint8_t txd_complete = TRUE;
volatile uint8_t ReceivedBytes = 0;
volatile uint8_t *pRxData = 0;
volatile uint8_t RxDataLen = 0;
 
uint8_t PcAccess = 100;
 
uint8_t MotorTest_Active  = 0;
uint8_t MotorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
uint8_t ConfirmFrame;
 
typedef struct
{
        int16_t Heading;
} __attribute__((packed)) Heading_t;
 
DebugOut_t              DebugOut;
Data3D_t                Data3D;
ExternControl_t ExternControl;
UART_VersionInfo_t      UART_VersionInfo;
 
uint16_t DebugData_Timer;
uint16_t Data3D_Timer;
uint16_t DebugData_Interval = 500; // in 1ms
uint16_t Data3D_Interval = 0; // in 1ms
 
#ifdef USE_MK3MAG
int16_t Compass_Timer;
#endif
 
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] =
{
   //1234567890123456
    "AngleNick       ", //0
    "AngleRoll       ",
    "AccNick         ",
    "AccRoll         ",
    "YawGyro         ",
    "Height Value    ", //5
    "AccZ            ",
    "Gas             ",
    "Compass Heading ",
    "Voltage         ",
    "Receiver Level  ", //10
    "YawGyro Heading ",
    "Motor Front     ",
    "Motor Rear      ",
    "Motor Left      ",
    "Motor Right     ", //15
    "                ",
    "                ",
    "                ",
    "CompassCalState ",
    "NickServo       ", //20
    "                ",
    "Act I [100mA]   ",
    "Used Cap [mAh]  ",
    "                ",
    "                ", //25
    "                ",
    "Kalman Max Drift",
    "                ",
    "Navi Serial Data",
    "GPS Nick        ", //30
    "GPSS Roll       "
};
 
 
 
/****************************************************************/
/*              Initialization of the USART0                    */
/****************************************************************/
void USART0_Init (void)
{
        uint8_t sreg = SREG;
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
 
        // disable all interrupts before configuration
        cli();
 
        // disable RX-Interrupt
        UCSR0B &= ~(1 << RXCIE0);
        // disable TX-Interrupt
        UCSR0B &= ~(1 << TXCIE0);
 
        // set direction of RXD0 and TXD0 pins
        // set RXD0 (PD0) as an input pin
        PORTD |= (1 << PORTD0);
        DDRD &= ~(1 << DDD0);
        // set TXD0 (PD1) as an output pin
        PORTD |= (1 << PORTD1);
        DDRD |=  (1 << DDD1);
 
        // USART0 Baud Rate Register
        // set clock divider
        UBRR0H = (uint8_t)(ubrr >> 8);
        UBRR0L = (uint8_t)ubrr;
 
        // USART0 Control and Status Register A, B, C
 
        // enable double speed operation in
        UCSR0A |= (1 << U2X0);
        // enable receiver and transmitter in
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
        // set asynchronous mode
        UCSR0C &= ~(1 << UMSEL01);
        UCSR0C &= ~(1 << UMSEL00);
        // no parity
        UCSR0C &= ~(1 << UPM01);
        UCSR0C &= ~(1 << UPM00);
        // 1 stop bit
        UCSR0C &= ~(1 << USBS0);
        // 8-bit
        UCSR0B &= ~(1 << UCSZ02);
        UCSR0C |=  (1 << UCSZ01);
        UCSR0C |=  (1 << UCSZ00);
 
                // flush receive buffer
        while ( UCSR0A & (1<<RXC0) ) UDR0;
 
        // enable interrupts at the end
        // enable RX-Interrupt
        UCSR0B |= (1 << RXCIE0);
        // enable TX-Interrupt
        UCSR0B |= (1 << TXCIE0);
 
        // initialize the debug timer
        DebugData_Timer = SetDelay(DebugData_Interval);
 
        // unlock rxd_buffer
        rxd_buffer_locked = FALSE;
        pRxData = 0;
        RxDataLen = 0;
 
        // no bytes to send
        txd_complete = TRUE;
 
        #ifdef USE_MK3MAG
        Compass_Timer = SetDelay(220);
        #endif
 
        UART_VersionInfo.SWMajor = VERSION_MAJOR;
        UART_VersionInfo.SWMinor = VERSION_MINOR;
        UART_VersionInfo.SWPatch = VERSION_PATCH;
        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
        UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
 
        // restore global interrupt flags
    SREG = sreg;
}
 
/****************************************************************/
/*               USART0 transmitter ISR                         */
/****************************************************************/
ISR(USART0_TX_vect)
{
        static uint16_t ptr_txd_buffer = 0;
        uint8_t tmp_tx;
        if(!txd_complete) // transmission not completed
        {
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
                tmp_tx = txd_buffer[ptr_txd_buffer];
                // if terminating character or end of txd buffer was reached
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
                {
                        ptr_txd_buffer = 0; // reset txd pointer
                        txd_complete = 1; // stop transmission
                }
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
        }
        // transmission completed
        else ptr_txd_buffer = 0;
}
 
/****************************************************************/
/*               USART0 receiver ISR                            */
/****************************************************************/
ISR(USART0_RX_vect)
{
        static uint16_t crc;
        static uint8_t ptr_rxd_buffer = 0;
        uint8_t crc1, crc2;
        uint8_t c;
 
        c = UDR0;  // catch the received byte
 
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
        // If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.
        if(CPUType != ATMEGA644P) ubx_parser(c);
        #endif
 
        if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
 
        // the rxd buffer is unlocked
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
        {
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
                crc = c; // init crc
        }
        #if 0
        else if (ptr_rxd_buffer == 1) // handle address
        {
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
                crc += c; // update crc
        }
        #endif
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
        {
                if(c != '\r') // no termination character
                {
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
                        crc += c; // update crc
                }
                else // termination character was received
                {
                        // the last 2 bytes are no subject for checksum calculation
                        // they are the checksum itself
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
                        // calculate checksum from transmitted data
                        crc %= 4096;
                        crc1 = '=' + crc / 64;
                        crc2 = '=' + crc % 64;
                        // compare checksum to transmitted checksum bytes
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
                        {   // checksum valid
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
                                ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
                        }
                        else
                        {       // checksum invalid
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
                        }
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
                }
        }
        else // rxd buffer overrun
        {
                ptr_rxd_buffer = 0; // reset rxd buffer
                rxd_buffer_locked = FALSE; // unlock rxd buffer
        }
 
}
 
 
// --------------------------------------------------------------------------
void AddCRC(uint16_t datalen)
{
        uint16_t tmpCRC = 0, i;
        for(i = 0; i < datalen; i++)
        {
                tmpCRC += txd_buffer[i];
        }
        tmpCRC %= 4096;
        txd_buffer[i++] = '=' + tmpCRC / 64;
        txd_buffer[i++] = '=' + tmpCRC % 64;
        txd_buffer[i++] = '\r';
        txd_complete = FALSE;
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
}
 
 
 
// --------------------------------------------------------------------------
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
{
        va_list ap;
        uint16_t pt = 0;
        uint8_t a,b,c;
        uint8_t ptr = 0;
 
        uint8_t *pdata = 0;
        int len = 0;
 
        txd_buffer[pt++] = '#';                 // Start character
        txd_buffer[pt++] = 'a' + addr;  // Address (a=0; b=1,...)
        txd_buffer[pt++] = cmd;                 // Command
 
        va_start(ap, numofbuffers);
        if(numofbuffers)
        {
                pdata = va_arg(ap, uint8_t*);
                len = va_arg(ap, int);
                ptr = 0;
                numofbuffers--;
        }
 
        while(len)
        {
                if(len)
                {
                        a = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers)
                        {
                                pdata = va_arg(ap, uint8_t*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else a = 0;
                if(len)
                {
                        b = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers)
                        {
                                pdata = va_arg(ap, uint8_t*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else b = 0;
                if(len)
                {
                        c = pdata[ptr++];
                        len--;
                        if((!len) && numofbuffers)
                        {
                                pdata = va_arg(ap, uint8_t*);
                                len = va_arg(ap, int);
                                ptr = 0;
                                numofbuffers--;
                        }
                }
                else c = 0;
                txd_buffer[pt++] = '=' + (a >> 2);
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
                txd_buffer[pt++] = '=' + ( c & 0x3f);
        }
        va_end(ap);
        AddCRC(pt); // add checksum after data block and initates the transmission
}
 
 
// --------------------------------------------------------------------------
void Decode64(void)
{
        uint8_t a,b,c,d;
        uint8_t x,y,z;
        uint8_t ptrIn = 3;
        uint8_t ptrOut = 3;
        uint8_t len = ReceivedBytes - 6;
 
        while(len)
        {
                a = rxd_buffer[ptrIn++] - '=';
                b = rxd_buffer[ptrIn++] - '=';
                c = rxd_buffer[ptrIn++] - '=';
                d = rxd_buffer[ptrIn++] - '=';
                //if(ptrIn > ReceivedBytes - 3) break;
 
                x = (a << 2) | (b >> 4);
                y = ((b & 0x0f) << 4) | (c >> 2);
                z = ((c & 0x03) << 6) | d;
 
                if(len--) rxd_buffer[ptrOut++] = x; else break;
                if(len--) rxd_buffer[ptrOut++] = y; else break;
                if(len--) rxd_buffer[ptrOut++] = z; else break;
        }
        pRxData = &rxd_buffer[3];
        RxDataLen = ptrOut - 3;
}
 
 
// --------------------------------------------------------------------------
void USART0_ProcessRxData(void)
{
        // if data in the rxd buffer are not locked immediately return
        if(!rxd_buffer_locked) return;
 
        uint8_t tempchar1, tempchar2;
 
        Decode64(); // decode data block in rxd_buffer
 
        switch(rxd_buffer[1] - 'a')
        {
                case FC_ADDRESS:
 
                switch(rxd_buffer[2])
                {
                        #ifdef USE_MK3MAG
                        case 'K':// compass value
                                CompassHeading = ((Heading_t *)pRxData)->Heading;
                                CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
                                break;
                        #endif
 
                        case 't':// motor test
                                if(RxDataLen > 20) //
                                {
                                        memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));
                                }
                                else
                                {
                                        memcpy(&MotorTest[0], (uint8_t*)pRxData, 4);
                                }
                                //Request_MotorTest = TRUE;
                                MotorTest_Active = 255;
                                PcAccess = 255;
                                break;
 
                        case 'n':// "Get Mixer Table
                                while(!txd_complete); // wait for previous frame to be sent
                                SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
                                break;
 
                        case 'm':// "Set Mixer Table
                                if(pRxData[0] == EEMIXER_REVISION)
                                {
                                        memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
                                        MixerTable_WriteToEEProm();
                                        while(!txd_complete); // wait for previous frame to be sent
                                        tempchar1 = 1;
                                }
                                else
                                {
                                        tempchar1 = 0;
                                }
                                SendOutData('M', FC_ADDRESS,  1, &tempchar1, 1);
                                break;
 
                        case 'p': // get PPM channels
                                Request_PPMChannels = TRUE;
                                break;
 
                        case 'q':// request settings
                                if(pRxData[0] == 0xFF)
                                {
                                        pRxData[0] = GetParamByte(PID_ACTIVE_SET);
                                }
                                // limit settings range
                                if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1
                                else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
                                // load requested parameter set
                                ParamSet_ReadFromEEProm(pRxData[0]);
                                tempchar1 = pRxData[0];
                                tempchar2 = EEPARAM_REVISION;
                                while(!txd_complete); // wait for previous frame to be sent
                                SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &ParamSet, sizeof(ParamSet));
                                break;
 
                        case 's': // save settings
                                if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors ar off
                                {
                                        if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings
                                        {
                                                memcpy(&ParamSet, (uint8_t*)&pRxData[2], sizeof(ParamSet));
                                                ParamSet_WriteToEEProm(pRxData[0]);
                                                TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
                                                TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
                                                tempchar1 = GetActiveParamSet();
                                                Beep(tempchar1);
                                        }
                                        else
                                        {
                                                tempchar1 = 0;  //indicate bad data
                                        }
                                        while(!txd_complete); // wait for previous frame to be sent
                                        SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
                                }
                                break;
 
                        default:
                                //unsupported command received
                                break;
                } // case FC_ADDRESS:
 
                default: // any Slave Address
 
                switch(rxd_buffer[2])
                {
                        case 'a':// request for labels of the analog debug outputs
                                Request_DebugLabel = pRxData[0];
                                if(Request_DebugLabel > 31) Request_DebugLabel = 31;
                                PcAccess = 255;
                                break;
 
                        case 'b': // submit extern control
                                memcpy(&ExternControl, (uint8_t*)pRxData, sizeof(ExternControl));
                                ConfirmFrame = ExternControl.Frame;
                                PcAccess = 255;
                                break;
 
                        case 'h':// request for display columns
                                PcAccess = 255;
                                RemoteKeys |= pRxData[0];
                                if(RemoteKeys) DisplayLine = 0;
                                Request_Display = TRUE;
                                break;
 
                        case 'l':// request for display columns
                                PcAccess = 255;
                                MenuItem = pRxData[0];
                                Request_Display1 = TRUE;
                                break;
 
                        case 'v': // request for version and board release
                                Request_VerInfo = TRUE;
                                break;
 
                        case 'g':// get external control data
                                Request_ExternalControl = TRUE;
                                break;
 
                        case 'd': // request for the debug data
                                DebugData_Interval = (uint16_t) pRxData[0] * 10;
                                if(DebugData_Interval > 0) Request_DebugData = TRUE;
                                break;
 
                        case 'c': // request for the 3D data
                                Data3D_Interval = (uint16_t) pRxData[0] * 10;
                                if(Data3D_Interval > 0) Request_Data3D = TRUE;
                                break;
 
                        default:
                                //unsupported command received
                                break;
                }
                break; // default:
        }
        // unlock the rxd buffer after processing
        pRxData = 0;
        RxDataLen = 0;
        rxd_buffer_locked = FALSE;
}
 
//############################################################################
//Routine für die Serielle Ausgabe
int16_t uart_putchar (int8_t c)
//############################################################################
{
        if (c == '\n')
                uart_putchar('\r');
        // wait until previous character was send
        loop_until_bit_is_set(UCSR0A, UDRE0);
        // send character
        UDR0 = c;
        return (0);
}
 
 
//---------------------------------------------------------------------------------------------
void USART0_TransmitTxData(void)
{
        if(!txd_complete) return;
 
        if(Request_VerInfo && txd_complete)
        {
                SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
                Request_VerInfo = FALSE;
        }
        if(Request_Display && txd_complete)
        {
                LCD_PrintMenu();
                SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
                DisplayLine++;
                if(DisplayLine >= 4) DisplayLine = 0;
                Request_Display = FALSE;
        }
        if(Request_Display1 && txd_complete)
        {
                LCD_PrintMenu();
                SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
                Request_Display1 = FALSE;
        }
        if(Request_DebugLabel != 0xFF) // Texte für die Analogdaten
        {
                uint8_t label[16]; // local sram buffer
                memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer
                SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16);
                Request_DebugLabel = 0xFF;
        }
        if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
        {
                SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
                ConfirmFrame = 0;
        }
        if( ((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete)
        {
                SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
                DebugData_Timer = SetDelay(DebugData_Interval);
                Request_DebugData = FALSE;
    }
    if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete)
        {
                SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
                Data3D.AngleNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D.AngleRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D.Heading   = (int16_t)((10 * YawGyroHeading)   / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
                Data3D_Timer = SetDelay(Data3D_Interval);
                Request_Data3D = FALSE;
    }
        if(Request_ExternalControl && txd_complete)
        {
                SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &ExternControl, sizeof(ExternControl));
                Request_ExternalControl = FALSE;
        }
 
        #ifdef USE_MK3MAG
    if((CheckDelay(Compass_Timer)) && txd_complete)
        {
                ToMk3Mag.Attitude[0] = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
                ToMk3Mag.Attitude[1] = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
                ToMk3Mag.CalState = CompassCalState;
                SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
                // the last state is 5 and should be send only once to avoid multiple flash writing
                if(CompassCalState > 4)  CompassCalState = 0;
                Compass_Timer = SetDelay(99);
        }
        #endif
 
        if(Request_MotorTest && txd_complete)
        {
                SendOutData('T', FC_ADDRESS, 0);
                Request_MotorTest = FALSE;
        }
        if(Request_PPMChannels && txd_complete)
        {
                SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in));
                Request_PPMChannels = FALSE;
        }
}
 
 

Rev 1227	Rev 1342
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + Nur für den privaten Gebrauch	3	// + Nur für den privaten Gebrauch
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt werden	18	// + eindeutig als Ursprung verlinkt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25	// + mit unserer Zustimmung zulässig	25	// + mit unserer Zustimmung zulässig
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30	// + this list of conditions and the following disclaimer.	30	// + this list of conditions and the following disclaimer.
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32	// + from this software without specific prior written permission.	32	// + from this software without specific prior written permission.
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34	// + for non-commercial use (directly or indirectly)	34	// + for non-commercial use (directly or indirectly)
35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36	// + with our written permission	36	// + with our written permission
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38	// + clearly linked as origin	38	// + clearly linked as origin
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49	// + POSSIBILITY OF SUCH DAMAGE.	49	// + POSSIBILITY OF SUCH DAMAGE.
50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51	#include <avr/io.h>	51	#include <avr/io.h>
52	#include <avr/interrupt.h>	52	#include <avr/interrupt.h>
53	#include <avr/wdt.h>	53	#include <avr/wdt.h>
54	#include <avr/pgmspace.h>	54	#include <avr/pgmspace.h>
55	#include <stdarg.h>	55	#include <stdarg.h>
56	#include <string.h>	56	#include <string.h>
57		57
58	#include "eeprom.h"	58	#include "eeprom.h"
59	#include "main.h"	59	#include "main.h"
60	#include "menu.h"	60	#include "menu.h"
61	#include "timer0.h"	61	#include "timer0.h"
62	#include "uart0.h"	62	#include "uart0.h"
63	#include "fc.h"	63	#include "fc.h"
64	#include "rc.h"	64	#include "rc.h"
65	#if defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG)	65	#if defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG)
66	#include "ubx.h"	66	#include "ubx.h"
67	#endif	67	#endif
68	#ifdef USE_MK3MAG	68	#ifdef USE_MK3MAG
69	#include "mk3mag.h"	69	#include "mk3mag.h"
70	#endif	70	#endif
71		71
72		72
73	#define FC_ADDRESS 1	73	#define FC_ADDRESS 1
74	#define NC_ADDRESS 2	74	#define NC_ADDRESS 2
75	#define MK3MAG_ADDRESS 3	75	#define MK3MAG_ADDRESS 3
76		76
77	#define FALSE 0	77	#define FALSE 0
78	#define TRUE 1	78	#define TRUE 1
79		79
80	//int8_t test __attribute__ ((section (".noinit")));	80	//int8_t test __attribute__ ((section (".noinit")));
81	uint8_t Request_VerInfo = FALSE;	81	uint8_t Request_VerInfo = FALSE;
82	uint8_t Request_ExternalControl = FALSE;	82	uint8_t Request_ExternalControl = FALSE;
83	uint8_t Request_Display = FALSE;	83	uint8_t Request_Display = FALSE;
84	uint8_t Request_Display1 = FALSE;	84	uint8_t Request_Display1 = FALSE;
85	uint8_t Request_DebugData = FALSE;	85	uint8_t Request_DebugData = FALSE;
86	uint8_t Request_Data3D = FALSE;	86	uint8_t Request_Data3D = FALSE;
87	uint8_t Request_DebugLabel = 255;	87	uint8_t Request_DebugLabel = 255;
88	uint8_t Request_PPMChannels = FALSE;	88	uint8_t Request_PPMChannels = FALSE;
89	uint8_t Request_MotorTest = FALSE;	89	uint8_t Request_MotorTest = FALSE;
90	uint8_t DisplayLine = 0;	90	uint8_t DisplayLine = 0;
91		91
92	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];	92	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
93	volatile uint8_t rxd_buffer_locked = FALSE;	93	volatile uint8_t rxd_buffer_locked = FALSE;
94	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];	94	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
95	volatile uint8_t txd_complete = TRUE;	95	volatile uint8_t txd_complete = TRUE;
96	volatile uint8_t ReceivedBytes = 0;	96	volatile uint8_t ReceivedBytes = 0;
97	volatile uint8_t *pRxData = 0;	97	volatile uint8_t *pRxData = 0;
98	volatile uint8_t RxDataLen = 0;	98	volatile uint8_t RxDataLen = 0;
99		99
100	uint8_t PcAccess = 100;	100	uint8_t PcAccess = 100;
101		101
102	uint8_t MotorTest_Active = 0;	102	uint8_t MotorTest_Active = 0;
103	uint8_t MotorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};	103	uint8_t MotorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
104	uint8_t ConfirmFrame;	104	uint8_t ConfirmFrame;
105		105
106	typedef struct	106	typedef struct
107	{	107	{
108	int16_t Heading;	108	int16_t Heading;
109	} __attribute__((packed)) Heading_t;	109	} __attribute__((packed)) Heading_t;
110		110
111	DebugOut_t DebugOut;	111	DebugOut_t DebugOut;
112	Data3D_t Data3D;	112	Data3D_t Data3D;
113	ExternControl_t ExternControl;	113	ExternControl_t ExternControl;
114	UART_VersionInfo_t UART_VersionInfo;	114	UART_VersionInfo_t UART_VersionInfo;
115		115
116	uint16_t DebugData_Timer;	116	uint16_t DebugData_Timer;
117	uint16_t Data3D_Timer;	117	uint16_t Data3D_Timer;
118	uint16_t DebugData_Interval = 500; // in 1ms	118	uint16_t DebugData_Interval = 500; // in 1ms
119	uint16_t Data3D_Interval = 0; // in 1ms	119	uint16_t Data3D_Interval = 0; // in 1ms
120		120
121	#ifdef USE_MK3MAG	121	#ifdef USE_MK3MAG
122	int16_t Compass_Timer;	122	int16_t Compass_Timer;
123	#endif	123	#endif
124		124
125	// keep lables in flash to save 512 bytes of sram space	125	// keep lables in flash to save 512 bytes of sram space
126	const prog_uint8_t ANALOG_LABEL[32][16] =	126	const prog_uint8_t ANALOG_LABEL[32][16] =
127	{	127	{
128	//1234567890123456	128	//1234567890123456
129	"AngleNick ", //0	129	"AngleNick ", //0
130	"AngleRoll ",	130	"AngleRoll ",
131	"AccNick ",	131	"AccNick ",
132	"AccRoll ",	132	"AccRoll ",
133	"YawGyro ",	133	"YawGyro ",
134	"Height Value ", //5	134	"Height Value ", //5
135	"AccZ ",	135	"AccZ ",
136	"Gas ",	136	"Gas ",
137	"Compass Heading ",	137	"Compass Heading ",
138	"Voltage ",	138	"Voltage ",
139	"Receiver Level ", //10	139	"Receiver Level ", //10
140	"YawGyro Heading ",	140	"YawGyro Heading ",
141	"Motor Front ",	141	"Motor Front ",
142	"Motor Rear ",	142	"Motor Rear ",
143	"Motor Left ",	143	"Motor Left ",
144	"Motor Right ", //15	144	"Motor Right ", //15
145	" ",	145	" ",
146	" ",	146	" ",
147	" ",	147	" ",
148	"CompassCalState ",	148	"CompassCalState ",
149	"NickServo ", //20	149	"NickServo ", //20
150	" ",	150	" ",
151	" ",	151	"Act I [100mA] ",
152	" ",	152	"Used Cap [mAh] ",
153	" ",	153	" ",
154	" ", //25	154	" ", //25
155	" ",	155	" ",
156	"Kalman Max Drift",	156	"Kalman Max Drift",
157	" ",	157	" ",
158	"Navi Serial Data",	158	"Navi Serial Data",
159	"GPS Nick ", //30	159	"GPS Nick ", //30
160	"GPSS Roll "	160	"GPSS Roll "
161	};	161	};
162		162
163		163
164		164
165	/****************************************************************/	165	/****************************************************************/
166	/* Initialization of the USART0 */	166	/* Initialization of the USART0 */
167	/****************************************************************/	167	/****************************************************************/
168	void USART0_Init (void)	168	void USART0_Init (void)
169	{	169	{
170	uint8_t sreg = SREG;	170	uint8_t sreg = SREG;
171	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);	171	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
172		172
173	// disable all interrupts before configuration	173	// disable all interrupts before configuration
174	cli();	174	cli();
175		175
176	// disable RX-Interrupt	176	// disable RX-Interrupt
177	UCSR0B &= ~(1 << RXCIE0);	177	UCSR0B &= ~(1 << RXCIE0);
178	// disable TX-Interrupt	178	// disable TX-Interrupt
179	UCSR0B &= ~(1 << TXCIE0);	179	UCSR0B &= ~(1 << TXCIE0);
180		180
181	// set direction of RXD0 and TXD0 pins	181	// set direction of RXD0 and TXD0 pins
182	// set RXD0 (PD0) as an input pin	182	// set RXD0 (PD0) as an input pin
183	PORTD \|= (1 << PORTD0);	183	PORTD \|= (1 << PORTD0);
184	DDRD &= ~(1 << DDD0);	184	DDRD &= ~(1 << DDD0);
185	// set TXD0 (PD1) as an output pin	185	// set TXD0 (PD1) as an output pin
186	PORTD \|= (1 << PORTD1);	186	PORTD \|= (1 << PORTD1);
187	DDRD \|= (1 << DDD1);	187	DDRD \|= (1 << DDD1);
188		188
189	// USART0 Baud Rate Register	189	// USART0 Baud Rate Register
190	// set clock divider	190	// set clock divider
191	UBRR0H = (uint8_t)(ubrr >> 8);	191	UBRR0H = (uint8_t)(ubrr >> 8);
192	UBRR0L = (uint8_t)ubrr;	192	UBRR0L = (uint8_t)ubrr;
193		193
194	// USART0 Control and Status Register A, B, C	194	// USART0 Control and Status Register A, B, C
195		195
196	// enable double speed operation in	196	// enable double speed operation in
197	UCSR0A \|= (1 << U2X0);	197	UCSR0A \|= (1 << U2X0);
198	// enable receiver and transmitter in	198	// enable receiver and transmitter in
199	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);	199	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);
200	// set asynchronous mode	200	// set asynchronous mode
201	UCSR0C &= ~(1 << UMSEL01);	201	UCSR0C &= ~(1 << UMSEL01);
202	UCSR0C &= ~(1 << UMSEL00);	202	UCSR0C &= ~(1 << UMSEL00);
203	// no parity	203	// no parity
204	UCSR0C &= ~(1 << UPM01);	204	UCSR0C &= ~(1 << UPM01);
205	UCSR0C &= ~(1 << UPM00);	205	UCSR0C &= ~(1 << UPM00);
206	// 1 stop bit	206	// 1 stop bit
207	UCSR0C &= ~(1 << USBS0);	207	UCSR0C &= ~(1 << USBS0);
208	// 8-bit	208	// 8-bit
209	UCSR0B &= ~(1 << UCSZ02);	209	UCSR0B &= ~(1 << UCSZ02);
210	UCSR0C \|= (1 << UCSZ01);	210	UCSR0C \|= (1 << UCSZ01);
211	UCSR0C \|= (1 << UCSZ00);	211	UCSR0C \|= (1 << UCSZ00);
212		212
213	// flush receive buffer	213	// flush receive buffer
214	while ( UCSR0A & (1<<RXC0) ) UDR0;	214	while ( UCSR0A & (1<<RXC0) ) UDR0;
215		215
216	// enable interrupts at the end	216	// enable interrupts at the end
217	// enable RX-Interrupt	217	// enable RX-Interrupt
218	UCSR0B \|= (1 << RXCIE0);	218	UCSR0B \|= (1 << RXCIE0);
219	// enable TX-Interrupt	219	// enable TX-Interrupt
220	UCSR0B \|= (1 << TXCIE0);	220	UCSR0B \|= (1 << TXCIE0);
221		221
222	// initialize the debug timer	222	// initialize the debug timer
223	DebugData_Timer = SetDelay(DebugData_Interval);	223	DebugData_Timer = SetDelay(DebugData_Interval);
224		224
225	// unlock rxd_buffer	225	// unlock rxd_buffer
226	rxd_buffer_locked = FALSE;	226	rxd_buffer_locked = FALSE;
227	pRxData = 0;	227	pRxData = 0;
228	RxDataLen = 0;	228	RxDataLen = 0;
229		229
230	// no bytes to send	230	// no bytes to send
231	txd_complete = TRUE;	231	txd_complete = TRUE;
232		232
233	#ifdef USE_MK3MAG	233	#ifdef USE_MK3MAG
234	Compass_Timer = SetDelay(220);	234	Compass_Timer = SetDelay(220);
235	#endif	235	#endif
236		236
237	UART_VersionInfo.SWMajor = VERSION_MAJOR;	237	UART_VersionInfo.SWMajor = VERSION_MAJOR;
238	UART_VersionInfo.SWMinor = VERSION_MINOR;	238	UART_VersionInfo.SWMinor = VERSION_MINOR;
239	UART_VersionInfo.SWPatch = VERSION_PATCH;	239	UART_VersionInfo.SWPatch = VERSION_PATCH;
240	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	240	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
241	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	241	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
242		242
243	// restore global interrupt flags	243	// restore global interrupt flags
244	SREG = sreg;	244	SREG = sreg;
245	}	245	}
246		246
247	/****************************************************************/	247	/****************************************************************/
248	/* USART0 transmitter ISR */	248	/* USART0 transmitter ISR */
249	/****************************************************************/	249	/****************************************************************/
250	ISR(USART0_TX_vect)	250	ISR(USART0_TX_vect)
251	{	251	{
252	static uint16_t ptr_txd_buffer = 0;	252	static uint16_t ptr_txd_buffer = 0;
253	uint8_t tmp_tx;	253	uint8_t tmp_tx;
254	if(!txd_complete) // transmission not completed	254	if(!txd_complete) // transmission not completed
255	{	255	{
256	ptr_txd_buffer++; // die [0] wurde schon gesendet	256	ptr_txd_buffer++; // die [0] wurde schon gesendet
257	tmp_tx = txd_buffer[ptr_txd_buffer];	257	tmp_tx = txd_buffer[ptr_txd_buffer];
258	// if terminating character or end of txd buffer was reached	258	// if terminating character or end of txd buffer was reached
259	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))	259	if((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN))
260	{	260	{
261	ptr_txd_buffer = 0; // reset txd pointer	261	ptr_txd_buffer = 0; // reset txd pointer
262	txd_complete = 1; // stop transmission	262	txd_complete = 1; // stop transmission
263	}	263	}
264	UDR0 = tmp_tx; // send current byte will trigger this ISR again	264	UDR0 = tmp_tx; // send current byte will trigger this ISR again
265	}	265	}
266	// transmission completed	266	// transmission completed
267	else ptr_txd_buffer = 0;	267	else ptr_txd_buffer = 0;
268	}	268	}
269		269
270	/****************************************************************/	270	/****************************************************************/
271	/* USART0 receiver ISR */	271	/* USART0 receiver ISR */
272	/****************************************************************/	272	/****************************************************************/
273	ISR(USART0_RX_vect)	273	ISR(USART0_RX_vect)
274	{	274	{
275	static uint16_t crc;	275	static uint16_t crc;
276	static uint8_t ptr_rxd_buffer = 0;	276	static uint8_t ptr_rxd_buffer = 0;
277	uint8_t crc1, crc2;	277	uint8_t crc1, crc2;
278	uint8_t c;	278	uint8_t c;
279		279
280	c = UDR0; // catch the received byte	280	c = UDR0; // catch the received byte
281		281
282	#if (defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG))	282	#if (defined (USE_KILLAGREG) \|\| defined (USE_MK3MAG))
283	// If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.	283	// If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.
284	if(CPUType != ATMEGA644P) ubx_parser(c);	284	if(CPUType != ATMEGA644P) ubx_parser(c);
285	#endif	285	#endif
286		286
287	if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return	287	if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
288		288
289	// the rxd buffer is unlocked	289	// the rxd buffer is unlocked
290	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received	290	if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
291	{	291	{
292	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	292	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
293	crc = c; // init crc	293	crc = c; // init crc
294	}	294	}
295	#if 0	295	#if 0
296	else if (ptr_rxd_buffer == 1) // handle address	296	else if (ptr_rxd_buffer == 1) // handle address
297	{	297	{
298	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	298	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
299	crc += c; // update crc	299	crc += c; // update crc
300	}	300	}
301	#endif	301	#endif
302	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes	302	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
303	{	303	{
304	if(c != '\r') // no termination character	304	if(c != '\r') // no termination character
305	{	305	{
306	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	306	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
307	crc += c; // update crc	307	crc += c; // update crc
308	}	308	}
309	else // termination character was received	309	else // termination character was received
310	{	310	{
311	// the last 2 bytes are no subject for checksum calculation	311	// the last 2 bytes are no subject for checksum calculation
312	// they are the checksum itself	312	// they are the checksum itself
313	crc -= rxd_buffer[ptr_rxd_buffer-2];	313	crc -= rxd_buffer[ptr_rxd_buffer-2];
314	crc -= rxd_buffer[ptr_rxd_buffer-1];	314	crc -= rxd_buffer[ptr_rxd_buffer-1];
315	// calculate checksum from transmitted data	315	// calculate checksum from transmitted data
316	crc %= 4096;	316	crc %= 4096;
317	crc1 = '=' + crc / 64;	317	crc1 = '=' + crc / 64;
318	crc2 = '=' + crc % 64;	318	crc2 = '=' + crc % 64;
319	// compare checksum to transmitted checksum bytes	319	// compare checksum to transmitted checksum bytes
320	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))	320	if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
321	{ // checksum valid	321	{ // checksum valid
322	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	322	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
323	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes	323	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
324	rxd_buffer_locked = TRUE; // lock the rxd buffer	324	rxd_buffer_locked = TRUE; // lock the rxd buffer
325	// if 2nd byte is an 'R' enable watchdog that will result in an reset	325	// if 2nd byte is an 'R' enable watchdog that will result in an reset
326	if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando	326	if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
327	}	327	}
328	else	328	else
329	{ // checksum invalid	329	{ // checksum invalid
330	rxd_buffer_locked = FALSE; // unlock rxd buffer	330	rxd_buffer_locked = FALSE; // unlock rxd buffer
331	}	331	}
332	ptr_rxd_buffer = 0; // reset rxd buffer pointer	332	ptr_rxd_buffer = 0; // reset rxd buffer pointer
333	}	333	}
334	}	334	}
335	else // rxd buffer overrun	335	else // rxd buffer overrun
336	{	336	{
337	ptr_rxd_buffer = 0; // reset rxd buffer	337	ptr_rxd_buffer = 0; // reset rxd buffer
338	rxd_buffer_locked = FALSE; // unlock rxd buffer	338	rxd_buffer_locked = FALSE; // unlock rxd buffer
339	}	339	}
340		340
341	}	341	}
342		342
343		343
344	// --------------------------------------------------------------------------	344	// --------------------------------------------------------------------------
345	void AddCRC(uint16_t datalen)	345	void AddCRC(uint16_t datalen)
346	{	346	{
347	uint16_t tmpCRC = 0, i;	347	uint16_t tmpCRC = 0, i;
348	for(i = 0; i < datalen; i++)	348	for(i = 0; i < datalen; i++)
349	{	349	{
350	tmpCRC += txd_buffer[i];	350	tmpCRC += txd_buffer[i];
351	}	351	}
352	tmpCRC %= 4096;	352	tmpCRC %= 4096;
353	txd_buffer[i++] = '=' + tmpCRC / 64;	353	txd_buffer[i++] = '=' + tmpCRC / 64;
354	txd_buffer[i++] = '=' + tmpCRC % 64;	354	txd_buffer[i++] = '=' + tmpCRC % 64;
355	txd_buffer[i++] = '\r';	355	txd_buffer[i++] = '\r';
356	txd_complete = FALSE;	356	txd_complete = FALSE;
357	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)	357	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
358	}	358	}
359		359
360		360
361		361
362	// --------------------------------------------------------------------------	362	// --------------------------------------------------------------------------
363	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...	363	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
364	{	364	{
365	va_list ap;	365	va_list ap;
366	uint16_t pt = 0;	366	uint16_t pt = 0;
367	uint8_t a,b,c;	367	uint8_t a,b,c;
368	uint8_t ptr = 0;	368	uint8_t ptr = 0;
369		369
370	uint8_t *pdata = 0;	370	uint8_t *pdata = 0;
371	int len = 0;	371	int len = 0;
372		372
373	txd_buffer[pt++] = '#'; // Start character	373	txd_buffer[pt++] = '#'; // Start character
374	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)	374	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
375	txd_buffer[pt++] = cmd; // Command	375	txd_buffer[pt++] = cmd; // Command
376		376
377	va_start(ap, numofbuffers);	377	va_start(ap, numofbuffers);
378	if(numofbuffers)	378	if(numofbuffers)
379	{	379	{
380	pdata = va_arg(ap, uint8_t*);	380	pdata = va_arg(ap, uint8_t*);
381	len = va_arg(ap, int);	381	len = va_arg(ap, int);
382	ptr = 0;	382	ptr = 0;
383	numofbuffers--;	383	numofbuffers--;
384	}	384	}
385		385
386	while(len)	386	while(len)
387	{	387	{
388	if(len)	388	if(len)
389	{	389	{
390	a = pdata[ptr++];	390	a = pdata[ptr++];
391	len--;	391	len--;
392	if((!len) && numofbuffers)	392	if((!len) && numofbuffers)
393	{	393	{
394	pdata = va_arg(ap, uint8_t*);	394	pdata = va_arg(ap, uint8_t*);
395	len = va_arg(ap, int);	395	len = va_arg(ap, int);
396	ptr = 0;	396	ptr = 0;
397	numofbuffers--;	397	numofbuffers--;
398	}	398	}
399	}	399	}
400	else a = 0;	400	else a = 0;
401	if(len)	401	if(len)
402	{	402	{
403	b = pdata[ptr++];	403	b = pdata[ptr++];
404	len--;	404	len--;
405	if((!len) && numofbuffers)	405	if((!len) && numofbuffers)
406	{	406	{
407	pdata = va_arg(ap, uint8_t*);	407	pdata = va_arg(ap, uint8_t*);
408	len = va_arg(ap, int);	408	len = va_arg(ap, int);
409	ptr = 0;	409	ptr = 0;
410	numofbuffers--;	410	numofbuffers--;
411	}	411	}
412	}	412	}
413	else b = 0;	413	else b = 0;
414	if(len)	414	if(len)
415	{	415	{
416	c = pdata[ptr++];	416	c = pdata[ptr++];
417	len--;	417	len--;
418	if((!len) && numofbuffers)	418	if((!len) && numofbuffers)
419	{	419	{
420	pdata = va_arg(ap, uint8_t*);	420	pdata = va_arg(ap, uint8_t*);
421	len = va_arg(ap, int);	421	len = va_arg(ap, int);
422	ptr = 0;	422	ptr = 0;
423	numofbuffers--;	423	numofbuffers--;
424	}	424	}
425	}	425	}
426	else c = 0;	426	else c = 0;
427	txd_buffer[pt++] = '=' + (a >> 2);	427	txd_buffer[pt++] = '=' + (a >> 2);
428	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	428	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
429	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	429	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
430	txd_buffer[pt++] = '=' + ( c & 0x3f);	430	txd_buffer[pt++] = '=' + ( c & 0x3f);
431	}	431	}
432	va_end(ap);	432	va_end(ap);
433	AddCRC(pt); // add checksum after data block and initates the transmission	433	AddCRC(pt); // add checksum after data block and initates the transmission
434	}	434	}
435		435
436		436
437	// --------------------------------------------------------------------------	437	// --------------------------------------------------------------------------
438	void Decode64(void)	438	void Decode64(void)
439	{	439	{
440	uint8_t a,b,c,d;	440	uint8_t a,b,c,d;
441	uint8_t x,y,z;	441	uint8_t x,y,z;
442	uint8_t ptrIn = 3;	442	uint8_t ptrIn = 3;
443	uint8_t ptrOut = 3;	443	uint8_t ptrOut = 3;
444	uint8_t len = ReceivedBytes - 6;	444	uint8_t len = ReceivedBytes - 6;
445		445
446	while(len)	446	while(len)
447	{	447	{
448	a = rxd_buffer[ptrIn++] - '=';	448	a = rxd_buffer[ptrIn++] - '=';
449	b = rxd_buffer[ptrIn++] - '=';	449	b = rxd_buffer[ptrIn++] - '=';
450	c = rxd_buffer[ptrIn++] - '=';	450	c = rxd_buffer[ptrIn++] - '=';
451	d = rxd_buffer[ptrIn++] - '=';	451	d = rxd_buffer[ptrIn++] - '=';
452	//if(ptrIn > ReceivedBytes - 3) break;	452	//if(ptrIn > ReceivedBytes - 3) break;
453		453
454	x = (a << 2) \| (b >> 4);	454	x = (a << 2) \| (b >> 4);
455	y = ((b & 0x0f) << 4) \| (c >> 2);	455	y = ((b & 0x0f) << 4) \| (c >> 2);
456	z = ((c & 0x03) << 6) \| d;	456	z = ((c & 0x03) << 6) \| d;
457		457
458	if(len--) rxd_buffer[ptrOut++] = x; else break;	458	if(len--) rxd_buffer[ptrOut++] = x; else break;
459	if(len--) rxd_buffer[ptrOut++] = y; else break;	459	if(len--) rxd_buffer[ptrOut++] = y; else break;
460	if(len--) rxd_buffer[ptrOut++] = z; else break;	460	if(len--) rxd_buffer[ptrOut++] = z; else break;
461	}	461	}
462	pRxData = &rxd_buffer[3];	462	pRxData = &rxd_buffer[3];
463	RxDataLen = ptrOut - 3;	463	RxDataLen = ptrOut - 3;
464	}	464	}
465		465
466		466
467	// --------------------------------------------------------------------------	467	// --------------------------------------------------------------------------
468	void USART0_ProcessRxData(void)	468	void USART0_ProcessRxData(void)
469	{	469	{
470	// if data in the rxd buffer are not locked immediately return	470	// if data in the rxd buffer are not locked immediately return
471	if(!rxd_buffer_locked) return;	471	if(!rxd_buffer_locked) return;
472		472
473	uint8_t tempchar1, tempchar2;	473	uint8_t tempchar1, tempchar2;
474		474
475	Decode64(); // decode data block in rxd_buffer	475	Decode64(); // decode data block in rxd_buffer
476		476
477	switch(rxd_buffer[1] - 'a')	477	switch(rxd_buffer[1] - 'a')
478	{	478	{
479	case FC_ADDRESS:	479	case FC_ADDRESS:
480		480
481	switch(rxd_buffer[2])	481	switch(rxd_buffer[2])
482	{	482	{
483	#ifdef USE_MK3MAG	483	#ifdef USE_MK3MAG
484	case 'K':// compass value	484	case 'K':// compass value
485	CompassHeading = ((Heading_t *)pRxData)->Heading;	485	CompassHeading = ((Heading_t *)pRxData)->Heading;
486	CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;	486	CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
487	break;	487	break;
488	#endif	488	#endif
489		489
490	case 't':// motor test	490	case 't':// motor test
491	if(RxDataLen > 20) //	491	if(RxDataLen > 20) //
492	{	492	{
493	memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));	493	memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));
494	}	494	}
495	else	495	else
496	{	496	{
497	memcpy(&MotorTest[0], (uint8_t*)pRxData, 4);	497	memcpy(&MotorTest[0], (uint8_t*)pRxData, 4);
498	}	498	}
499	//Request_MotorTest = TRUE;	499	//Request_MotorTest = TRUE;
500	MotorTest_Active = 255;	500	MotorTest_Active = 255;

Subversion Repositories FlightCtrl

(root)/branches/V0.74d_CRD_killagreg_Batteriekappazität_Lion/uart0.c - Rev 1227 → 1342