WebSVN - FlightCtrl - Diff - Rev 1910 and 1927 - /branches/dongfang_FC


#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 "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.h"
 
 
#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_OutputTest = FALSE;
uint8_t request_variables = 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 outputTestActive = 0;
uint8_t outputTest[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;
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
                "AnglePitch      ", //0
                "AngleRoll       ",
                "AngleYaw        ",
                "GyroPitch(PID)  ",
                "GyroRoll(PID)   ",
                "GyroYaw         ", //5
                "GyroFactorPitch ",
                "GyroFactorRoll  ",
                "GyroFactorYaw   ",
                "GyroDPitch      ",
                "GyroDRoll       ",//10
                "GyroDYaw        ",
                "Pitch Term      ",
                "Roll Term       ",
                "Throttle Term   ",
                "Yaw Term        ", //15
                "0th O Corr pitch",
                "0th O Corr roll ",
                "ControlIntePitch",
                "ControlInteRoll ",
                "UBat            ", //20
                "hoverThrottle   ",
                "IPartPitch      ", // OK
                "IPartRoll       ",
                "S3 (THROTTLE)   ",
                "S4 (RUDDER)     ", // OK //25
                "ControlYaw      ",
                "Airpress. Range ", // OK
                "DriftCompPitch  ",
                "DriftCompRoll   ",
                "AirpressFiltered", // MISSING //30
                "AirpressADC     " };
 
/****************************************************************/
/*              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;
 
        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 (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)
}
 
// --------------------------------------------------------------------------
// application example:
// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
/*
 void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
 va_list ap;
 uint16_t txd_bufferIndex = 0;
 uint8_t *currentBuffer;
 uint8_t currentBufferIndex;
 uint16_t lengthOfCurrentBuffer;
 uint8_t shift = 0;
 
 txd_buffer[txd_bufferIndex++] = '#';                   // Start character
 txd_buffer[txd_bufferIndex++] = 'a' + addr;            // Address (a=0; b=1,...)
 txd_buffer[txd_bufferIndex++] = cmd;                   // Command
 
 va_start(ap, numofbuffers);
 
 while(numofbuffers) {
 currentBuffer = va_arg(ap, uint8_t*);
 lengthOfCurrentBuffer = va_arg(ap, int);
 currentBufferIndex = 0;
 // Encode data: 3 bytes of data are encoded into 4 bytes,
 // where the 2 most significant bits are both 0.
 while(currentBufferIndex != lengthOfCurrentBuffer) {
 if (!shift) txd_buffer[txd_bufferIndex] = 0;
 txd_buffer[txd_bufferIndex]  |= currentBuffer[currentBufferIndex] >> (shift + 2);
 txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
 shift += 2;
 if (shift == 6) { shift=0; txd_bufferIndex++; }
 currentBufferIndex++;
 }
 }
 // If the number of data bytes was not divisible by 3, stuff
 //  with 0 pseudodata  until length is again divisible by 3.
 if (shift == 2) {
 // We need to stuff with zero bytes at the end.
 txd_buffer[txd_bufferIndex]  &= 0b00110000;
 txd_buffer[++txd_bufferIndex] = 0;
 shift = 4;
 }
 if (shift == 4) {
 // We need to stuff with zero bytes at the end.
 txd_buffer[txd_bufferIndex++] &= 0b00111100;
 txd_buffer[txd_bufferIndex]    = 0;
 }
 va_end(ap);
 AddCRC(pt); // add checksum after data block and initates the transmission
 }
 */
 
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) {
        // We control the outputTestActive var from here: Count it down.
        if (outputTestActive)
                outputTestActive--;
        // 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(&outputTest[0], (uint8_t*) pRxData, sizeof(outputTest));
                        } else {
                                memcpy(&outputTest[0], (uint8_t*) pRxData, 4);
                        }
                        outputTestActive = 255;
                        externalControlActive = 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();
                        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 *) &staticParams,
                                        sizeof(staticParams));
                        break;
 
                case 's': // save settings
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
                        {
                                if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1]
                                                == EEPARAM_REVISION)) // check for setting to be in range and version of settings
                                {
                                        memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams));
                                        ParamSet_WriteToEEProm();
 
                                        beepNumber(1);
                                } 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;
                        externalControlActive = 255;
                        break;
 
                case 'b': // submit extern control
                        memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
                        ConfirmFrame = externalControl.frame;
                        externalControlActive = 255;
                        break;
 
                case 'h':// request for display columns
                  externalControlActive = 255;
                        request_Display = TRUE;
                        break;
 
                case 'l':// request for display columns
                  externalControlActive = 255;
                        request_Display1 = TRUE;
                        break;
 
                case 'v': // request for version and board release
                        request_VerInfo = TRUE;
                        break;
 
                case 'x':
                        request_variables = 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, 0);
                DisplayLine++;
                if (DisplayLine >= 4)
                        DisplayLine = 0;
                request_Display = FALSE;
        }
 
        if (request_Display1 && txd_complete) {
          //    LCD_PrintMenu();
                SendOutData('L', FC_ADDRESS, 0);
                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 * angle[PITCH])
                                / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
                Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL])
                                / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
                Data3D.Heading = (int16_t) ((10 * angle[YAW]) / GYRO_DEG_FACTOR_YAW); // 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 *) &externalControl,
                                sizeof(externalControl));
                request_ExternalControl = FALSE;
        }
 
 
#ifdef USE_MK3MAG
        if((checkDelay(Compass_Timer)) && txd_complete) {
                ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
                ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
                ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
                ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
                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_OutputTest && txd_complete) {
                SendOutData('T', FC_ADDRESS, 0);
                request_OutputTest = FALSE;
        }
 
        if (request_PPMChannels && txd_complete) {
                SendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in));
                request_PPMChannels = FALSE;
        }
 
        if (request_variables && txd_complete) {
                SendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
                request_variables = FALSE;
        }
}
 

Rev 1910	Rev 1927
1	#include <avr/io.h>	1	#include <avr/io.h>
2	#include <avr/interrupt.h>	2	#include <avr/interrupt.h>
3	#include <avr/wdt.h>	3	#include <avr/wdt.h>
4	#include <avr/pgmspace.h>	4	#include <avr/pgmspace.h>
5	#include <stdarg.h>	5	#include <stdarg.h>
6	#include <string.h>	6	#include <string.h>
7		7
8	#include "eeprom.h"	8	#include "eeprom.h"
9	#include "timer0.h"	9	#include "timer0.h"
10	#include "uart0.h"	10	#include "uart0.h"
11	#include "rc.h"	11	#include "rc.h"
12	#include "externalControl.h"	12	#include "externalControl.h"
13	#include "output.h"	13	#include "output.h"
14	#include "attitude.h"	14	#include "attitude.h"
15		15
16		16
17	#ifdef USE_MK3MAG	17	#ifdef USE_MK3MAG
18	#include "mk3mag.h"	18	#include "mk3mag.h"
19	#endif	19	#endif
20		20
21	#define FC_ADDRESS 1	21	#define FC_ADDRESS 1
22	#define NC_ADDRESS 2	22	#define NC_ADDRESS 2
23	#define MK3MAG_ADDRESS 3	23	#define MK3MAG_ADDRESS 3
24		24
25	#define FALSE 0	25	#define FALSE 0
26	#define TRUE 1	26	#define TRUE 1
27	//int8_t test __attribute__ ((section (".noinit")));	27	//int8_t test __attribute__ ((section (".noinit")));
28	uint8_t request_VerInfo = FALSE;	28	uint8_t request_VerInfo = FALSE;
29	uint8_t request_ExternalControl = FALSE;	29	uint8_t request_ExternalControl = FALSE;
30	uint8_t request_Display = FALSE;	30	uint8_t request_Display = FALSE;
31	uint8_t request_Display1 = FALSE;	31	uint8_t request_Display1 = FALSE;
32	uint8_t request_DebugData = FALSE;	32	uint8_t request_DebugData = FALSE;
33	uint8_t request_Data3D = FALSE;	33	uint8_t request_Data3D = FALSE;
34	uint8_t request_DebugLabel = 255;	34	uint8_t request_DebugLabel = 255;
35	uint8_t request_PPMChannels = FALSE;	35	uint8_t request_PPMChannels = FALSE;
36	uint8_t request_OutputTest = FALSE;	36	uint8_t request_OutputTest = FALSE;
37	uint8_t request_variables = FALSE;	37	uint8_t request_variables = FALSE;
38		38
39	uint8_t DisplayLine = 0;	39	uint8_t DisplayLine = 0;
40		40
41	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];	41	volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
42	volatile uint8_t rxd_buffer_locked = FALSE;	42	volatile uint8_t rxd_buffer_locked = FALSE;
43	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];	43	volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
44	volatile uint8_t txd_complete = TRUE;	44	volatile uint8_t txd_complete = TRUE;
45	volatile uint8_t ReceivedBytes = 0;	45	volatile uint8_t ReceivedBytes = 0;
46	volatile uint8_t *pRxData = 0;	46	volatile uint8_t *pRxData = 0;
47	volatile uint8_t RxDataLen = 0;	47	volatile uint8_t RxDataLen = 0;
48		48
49	uint8_t outputTestActive = 0;	49	uint8_t outputTestActive = 0;
50	uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };	50	uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
51	uint8_t ConfirmFrame;	51	uint8_t ConfirmFrame;
52		52
53	typedef struct {	53	typedef struct {
54	int16_t Heading;	54	int16_t Heading;
55	}__attribute__((packed)) Heading_t;	55	}__attribute__((packed)) Heading_t;
56		56
57	DebugOut_t DebugOut;	57	DebugOut_t DebugOut;
58	Data3D_t Data3D;	58	Data3D_t Data3D;
59	UART_VersionInfo_t UART_VersionInfo;	59	UART_VersionInfo_t UART_VersionInfo;
60		60
61	uint16_t DebugData_Timer;	61	uint16_t DebugData_Timer;
62	uint16_t Data3D_Timer;	62	uint16_t Data3D_Timer;
63	uint16_t DebugData_Interval = 500; // in 1ms	63	uint16_t DebugData_Interval = 500; // in 1ms
64	uint16_t Data3D_Interval = 0; // in 1ms	64	uint16_t Data3D_Interval = 0; // in 1ms
65		65
66	#ifdef USE_MK3MAG	66	#ifdef USE_MK3MAG
67	int16_t Compass_Timer;	67	int16_t Compass_Timer;
68	#endif	68	#endif
69		69
70	// keep lables in flash to save 512 bytes of sram space	70	// keep lables in flash to save 512 bytes of sram space
71	const prog_uint8_t ANALOG_LABEL[32][16] = {	71	const prog_uint8_t ANALOG_LABEL[32][16] = {
72	//1234567890123456	72	//1234567890123456
73	"AnglePitch ", //0	73	"AnglePitch ", //0
74	"AngleRoll ",	74	"AngleRoll ",
75	"AngleYaw ",	75	"AngleYaw ",
76	"GyroPitch(PID) ",	76	"GyroPitch(PID) ",
77	"GyroRoll(PID) ",	77	"GyroRoll(PID) ",
78	"GyroYaw ", //5	78	"GyroYaw ", //5
79	"GyroFactorPitch ",	79	"GyroFactorPitch ",
80	"GyroFactorRoll ",	80	"GyroFactorRoll ",
81	"GyroFactorYaw ",	81	"GyroFactorYaw ",
82	"GyroDPitch ",	82	"GyroDPitch ",
83	"GyroDRoll ",//10	83	"GyroDRoll ",//10
84	"GyroDYaw ",	84	"GyroDYaw ",
85	"Pitch Term ",	85	"Pitch Term ",
86	"Roll Term ",	86	"Roll Term ",
87	"Throttle Term ",	87	"Throttle Term ",
88	"Yaw Term ", //15	88	"Yaw Term ", //15
89	"0th O Corr pitch",	89	"0th O Corr pitch",
90	"0th O Corr roll ",	90	"0th O Corr roll ",
91	"ControlIntePitch",	91	"ControlIntePitch",
92	"ControlInteRoll ",	92	"ControlInteRoll ",
93	"UBat ", //20	93	"UBat ", //20
94	"hoverThrottle ",	94	"hoverThrottle ",
95	"IPartPitch ", // OK	95	"IPartPitch ", // OK
96	"IPartRoll ",	96	"IPartRoll ",
97	"S3 (THROTTLE) ",	97	"S3 (THROTTLE) ",
98	"S4 (RUDDER) ", // OK //25	98	"S4 (RUDDER) ", // OK //25
99	"ControlYaw ",	99	"ControlYaw ",
100	"Airpress. Range ", // OK	100	"Airpress. Range ", // OK
101	"DriftCompPitch ",	101	"DriftCompPitch ",
102	"DriftCompRoll ",	102	"DriftCompRoll ",
103	"AirpressFiltered", // MISSING //30	103	"AirpressFiltered", // MISSING //30
104	"AirpressADC " };	104	"AirpressADC " };
105		105
106	/****************************************************************/	106	/****************************************************************/
107	/* Initialization of the USART0 */	107	/* Initialization of the USART0 */
108	/****************************************************************/	108	/****************************************************************/
109	void usart0_Init(void) {	109	void usart0_Init(void) {
110	uint8_t sreg = SREG;	110	uint8_t sreg = SREG;
111	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);	111	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
112		112
113	// disable all interrupts before configuration	113	// disable all interrupts before configuration
114	cli();	114	cli();
115		115
116	// disable RX-Interrupt	116	// disable RX-Interrupt
117	UCSR0B &= ~(1 << RXCIE0);	117	UCSR0B &= ~(1 << RXCIE0);
118	// disable TX-Interrupt	118	// disable TX-Interrupt
119	UCSR0B &= ~(1 << TXCIE0);	119	UCSR0B &= ~(1 << TXCIE0);
120		120
121	// set direction of RXD0 and TXD0 pins	121	// set direction of RXD0 and TXD0 pins
122	// set RXD0 (PD0) as an input pin	122	// set RXD0 (PD0) as an input pin
123	PORTD \|= (1 << PORTD0);	123	PORTD \|= (1 << PORTD0);
124	DDRD &= ~(1 << DDD0);	124	DDRD &= ~(1 << DDD0);
125	// set TXD0 (PD1) as an output pin	125	// set TXD0 (PD1) as an output pin
126	PORTD \|= (1 << PORTD1);	126	PORTD \|= (1 << PORTD1);
127	DDRD \|= (1 << DDD1);	127	DDRD \|= (1 << DDD1);
128		128
129	// USART0 Baud Rate Register	129	// USART0 Baud Rate Register
130	// set clock divider	130	// set clock divider
131	UBRR0H = (uint8_t) (ubrr >> 8);	131	UBRR0H = (uint8_t) (ubrr >> 8);
132	UBRR0L = (uint8_t) ubrr;	132	UBRR0L = (uint8_t) ubrr;
133		133
134	// USART0 Control and Status Register A, B, C	134	// USART0 Control and Status Register A, B, C
135		135
136	// enable double speed operation in	136	// enable double speed operation in
137	UCSR0A \|= (1 << U2X0);	137	UCSR0A \|= (1 << U2X0);
138	// enable receiver and transmitter in	138	// enable receiver and transmitter in
139	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);	139	UCSR0B = (1 << TXEN0) \| (1 << RXEN0);
140	// set asynchronous mode	140	// set asynchronous mode
141	UCSR0C &= ~(1 << UMSEL01);	141	UCSR0C &= ~(1 << UMSEL01);
142	UCSR0C &= ~(1 << UMSEL00);	142	UCSR0C &= ~(1 << UMSEL00);
143	// no parity	143	// no parity
144	UCSR0C &= ~(1 << UPM01);	144	UCSR0C &= ~(1 << UPM01);
145	UCSR0C &= ~(1 << UPM00);	145	UCSR0C &= ~(1 << UPM00);
146	// 1 stop bit	146	// 1 stop bit
147	UCSR0C &= ~(1 << USBS0);	147	UCSR0C &= ~(1 << USBS0);
148	// 8-bit	148	// 8-bit
149	UCSR0B &= ~(1 << UCSZ02);	149	UCSR0B &= ~(1 << UCSZ02);
150	UCSR0C \|= (1 << UCSZ01);	150	UCSR0C \|= (1 << UCSZ01);
151	UCSR0C \|= (1 << UCSZ00);	151	UCSR0C \|= (1 << UCSZ00);
152		152
153	// flush receive buffer	153	// flush receive buffer
154	while (UCSR0A & (1 << RXC0))	154	while (UCSR0A & (1 << RXC0))
155	UDR0;	155	UDR0;
156		156
157	// enable interrupts at the end	157	// enable interrupts at the end
158	// enable RX-Interrupt	158	// enable RX-Interrupt
159	UCSR0B \|= (1 << RXCIE0);	159	UCSR0B \|= (1 << RXCIE0);
160	// enable TX-Interrupt	160	// enable TX-Interrupt
161	UCSR0B \|= (1 << TXCIE0);	161	UCSR0B \|= (1 << TXCIE0);
162		162
163	// initialize the debug timer	163	// initialize the debug timer
164	DebugData_Timer = setDelay(DebugData_Interval);	164	DebugData_Timer = setDelay(DebugData_Interval);
165		165
166	// unlock rxd_buffer	166	// unlock rxd_buffer
167	rxd_buffer_locked = FALSE;	167	rxd_buffer_locked = FALSE;
168	pRxData = 0;	168	pRxData = 0;
169	RxDataLen = 0;	169	RxDataLen = 0;
170		170
171	// no bytes to send	171	// no bytes to send
172	txd_complete = TRUE;	172	txd_complete = TRUE;
173		173
174	UART_VersionInfo.SWMajor = VERSION_MAJOR;	174	UART_VersionInfo.SWMajor = VERSION_MAJOR;
175	UART_VersionInfo.SWMinor = VERSION_MINOR;	175	UART_VersionInfo.SWMinor = VERSION_MINOR;
176	UART_VersionInfo.SWPatch = VERSION_PATCH;	176	UART_VersionInfo.SWPatch = VERSION_PATCH;
177	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;	177	UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
178	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;	178	UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
179		179
180	// restore global interrupt flags	180	// restore global interrupt flags
181	SREG = sreg;	181	SREG = sreg;
182	}	182	}
183		183
184	/****************************************************************/	184	/****************************************************************/
185	/* USART0 transmitter ISR */	185	/* USART0 transmitter ISR */
186	/****************************************************************/	186	/****************************************************************/
187	ISR(USART0_TX_vect)	187	ISR(USART0_TX_vect)
188	{	188	{
189	static uint16_t ptr_txd_buffer = 0;	189	static uint16_t ptr_txd_buffer = 0;
190	uint8_t tmp_tx;	190	uint8_t tmp_tx;
191	if (!txd_complete) { // transmission not completed	191	if (!txd_complete) { // transmission not completed
192	ptr_txd_buffer++; // die [0] wurde schon gesendet	192	ptr_txd_buffer++; // die [0] wurde schon gesendet
193	tmp_tx = txd_buffer[ptr_txd_buffer];	193	tmp_tx = txd_buffer[ptr_txd_buffer];
194	// if terminating character or end of txd buffer was reached	194	// if terminating character or end of txd buffer was reached
195	if ((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {	195	if ((tmp_tx == '\r') \|\| (ptr_txd_buffer == TXD_BUFFER_LEN)) {
196	ptr_txd_buffer = 0; // reset txd pointer	196	ptr_txd_buffer = 0; // reset txd pointer
197	txd_complete = 1; // stop transmission	197	txd_complete = 1; // stop transmission
198	}	198	}
199	UDR0 = tmp_tx; // send current byte will trigger this ISR again	199	UDR0 = tmp_tx; // send current byte will trigger this ISR again
200	}	200	}
201	// transmission completed	201	// transmission completed
202	else	202	else
203	ptr_txd_buffer = 0;	203	ptr_txd_buffer = 0;
204	}	204	}
205		205
206	/****************************************************************/	206	/****************************************************************/
207	/* USART0 receiver ISR */	207	/* USART0 receiver ISR */
208	/****************************************************************/	208	/****************************************************************/
209	ISR(USART0_RX_vect)	209	ISR(USART0_RX_vect)
210	{	210	{
211	static uint16_t crc;	211	static uint16_t crc;
212	static uint8_t ptr_rxd_buffer = 0;	212	static uint8_t ptr_rxd_buffer = 0;
213	uint8_t crc1, crc2;	213	uint8_t crc1, crc2;
214	uint8_t c;	214	uint8_t c;
215		215
216	c = UDR0; // catch the received byte	216	c = UDR0; // catch the received byte
217		217
218	if (rxd_buffer_locked)	218	if (rxd_buffer_locked)
219	return; // if rxd buffer is locked immediately return	219	return; // if rxd buffer is locked immediately return
220		220
221	// the rxd buffer is unlocked	221	// the rxd buffer is unlocked
222	if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received	222	if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
223	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer	223	rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
224	crc = c; // init crc	224	crc = c; // init crc
225	}	225	}
226	#if 0	226	#if 0
227	else if (ptr_rxd_buffer == 1) { // handle address	227	else if (ptr_rxd_buffer == 1) { // handle address
228	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	228	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
229	crc += c; // update crc	229	crc += c; // update crc
230	}	230	}
231	#endif	231	#endif
232	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes	232	else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
233	if (c != '\r') { // no termination character	233	if (c != '\r') { // no termination character
234	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer	234	rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
235	crc += c; // update crc	235	crc += c; // update crc
236	} else { // termination character was received	236	} else { // termination character was received
237	// the last 2 bytes are no subject for checksum calculation	237	// the last 2 bytes are no subject for checksum calculation
238	// they are the checksum itself	238	// they are the checksum itself
239	crc -= rxd_buffer[ptr_rxd_buffer - 2];	239	crc -= rxd_buffer[ptr_rxd_buffer - 2];
240	crc -= rxd_buffer[ptr_rxd_buffer - 1];	240	crc -= rxd_buffer[ptr_rxd_buffer - 1];
241	// calculate checksum from transmitted data	241	// calculate checksum from transmitted data
242	crc %= 4096;	242	crc %= 4096;
243	crc1 = '=' + crc / 64;	243	crc1 = '=' + crc / 64;
244	crc2 = '=' + crc % 64;	244	crc2 = '=' + crc % 64;
245	// compare checksum to transmitted checksum bytes	245	// compare checksum to transmitted checksum bytes
246	if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2	246	if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2
247	== rxd_buffer[ptr_rxd_buffer - 1])) {	247	== rxd_buffer[ptr_rxd_buffer - 1])) {
248	// checksum valid	248	// checksum valid
249	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character	249	rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
250	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes	250	ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
251	rxd_buffer_locked = TRUE; // lock the rxd buffer	251	rxd_buffer_locked = TRUE; // lock the rxd buffer
252	// if 2nd byte is an 'R' enable watchdog that will result in an reset	252	// if 2nd byte is an 'R' enable watchdog that will result in an reset
253	if (rxd_buffer[2] == 'R') {	253	if (rxd_buffer[2] == 'R') {
254	wdt_enable(WDTO_250MS);	254	wdt_enable(WDTO_250MS);
255	} // Reset-Commando	255	} // Reset-Commando
256	} else { // checksum invalid	256	} else { // checksum invalid
257	rxd_buffer_locked = FALSE; // unlock rxd buffer	257	rxd_buffer_locked = FALSE; // unlock rxd buffer
258	}	258	}
259	ptr_rxd_buffer = 0; // reset rxd buffer pointer	259	ptr_rxd_buffer = 0; // reset rxd buffer pointer
260	}	260	}
261	} else { // rxd buffer overrun	261	} else { // rxd buffer overrun
262	ptr_rxd_buffer = 0; // reset rxd buffer	262	ptr_rxd_buffer = 0; // reset rxd buffer
263	rxd_buffer_locked = FALSE; // unlock rxd buffer	263	rxd_buffer_locked = FALSE; // unlock rxd buffer
264	}	264	}
265	}	265	}
266		266
267	// --------------------------------------------------------------------------	267	// --------------------------------------------------------------------------
268	void AddCRC(uint16_t datalen) {	268	void AddCRC(uint16_t datalen) {
269	uint16_t tmpCRC = 0, i;	269	uint16_t tmpCRC = 0, i;
270	for (i = 0; i < datalen; i++) {	270	for (i = 0; i < datalen; i++) {
271	tmpCRC += txd_buffer[i];	271	tmpCRC += txd_buffer[i];
272	}	272	}
273	tmpCRC %= 4096;	273	tmpCRC %= 4096;
274	txd_buffer[i++] = '=' + tmpCRC / 64;	274	txd_buffer[i++] = '=' + tmpCRC / 64;
275	txd_buffer[i++] = '=' + tmpCRC % 64;	275	txd_buffer[i++] = '=' + tmpCRC % 64;
276	txd_buffer[i++] = '\r';	276	txd_buffer[i++] = '\r';
277	txd_complete = FALSE;	277	txd_complete = FALSE;
278	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)	278	UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
279	}	279	}
280		280
281	// --------------------------------------------------------------------------	281	// --------------------------------------------------------------------------
282	// application example:	282	// application example:
283	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);	283	// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
284	/*	284	/*
285	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	285	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
286	va_list ap;	286	va_list ap;
287	uint16_t txd_bufferIndex = 0;	287	uint16_t txd_bufferIndex = 0;
288	uint8_t *currentBuffer;	288	uint8_t *currentBuffer;
289	uint8_t currentBufferIndex;	289	uint8_t currentBufferIndex;
290	uint16_t lengthOfCurrentBuffer;	290	uint16_t lengthOfCurrentBuffer;
291	uint8_t shift = 0;	291	uint8_t shift = 0;
292		292
293	txd_buffer[txd_bufferIndex++] = '#'; // Start character	293	txd_buffer[txd_bufferIndex++] = '#'; // Start character
294	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)	294	txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)
295	txd_buffer[txd_bufferIndex++] = cmd; // Command	295	txd_buffer[txd_bufferIndex++] = cmd; // Command
296		296
297	va_start(ap, numofbuffers);	297	va_start(ap, numofbuffers);
298		298
299	while(numofbuffers) {	299	while(numofbuffers) {
300	currentBuffer = va_arg(ap, uint8_t*);	300	currentBuffer = va_arg(ap, uint8_t*);
301	lengthOfCurrentBuffer = va_arg(ap, int);	301	lengthOfCurrentBuffer = va_arg(ap, int);
302	currentBufferIndex = 0;	302	currentBufferIndex = 0;
303	// Encode data: 3 bytes of data are encoded into 4 bytes,	303	// Encode data: 3 bytes of data are encoded into 4 bytes,
304	// where the 2 most significant bits are both 0.	304	// where the 2 most significant bits are both 0.
305	while(currentBufferIndex != lengthOfCurrentBuffer) {	305	while(currentBufferIndex != lengthOfCurrentBuffer) {
306	if (!shift) txd_buffer[txd_bufferIndex] = 0;	306	if (!shift) txd_buffer[txd_bufferIndex] = 0;
307	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);	307	txd_buffer[txd_bufferIndex] \|= currentBuffer[currentBufferIndex] >> (shift + 2);
308	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;	308	txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
309	shift += 2;	309	shift += 2;
310	if (shift == 6) { shift=0; txd_bufferIndex++; }	310	if (shift == 6) { shift=0; txd_bufferIndex++; }
311	currentBufferIndex++;	311	currentBufferIndex++;
312	}	312	}
313	}	313	}
314	// If the number of data bytes was not divisible by 3, stuff	314	// If the number of data bytes was not divisible by 3, stuff
315	// with 0 pseudodata until length is again divisible by 3.	315	// with 0 pseudodata until length is again divisible by 3.
316	if (shift == 2) {	316	if (shift == 2) {
317	// We need to stuff with zero bytes at the end.	317	// We need to stuff with zero bytes at the end.
318	txd_buffer[txd_bufferIndex] &= 0b00110000;	318	txd_buffer[txd_bufferIndex] &= 0b00110000;
319	txd_buffer[++txd_bufferIndex] = 0;	319	txd_buffer[++txd_bufferIndex] = 0;
320	shift = 4;	320	shift = 4;
321	}	321	}
322	if (shift == 4) {	322	if (shift == 4) {
323	// We need to stuff with zero bytes at the end.	323	// We need to stuff with zero bytes at the end.
324	txd_buffer[txd_bufferIndex++] &= 0b00111100;	324	txd_buffer[txd_bufferIndex++] &= 0b00111100;
325	txd_buffer[txd_bufferIndex] = 0;	325	txd_buffer[txd_bufferIndex] = 0;
326	}	326	}
327	va_end(ap);	327	va_end(ap);
328	AddCRC(pt); // add checksum after data block and initates the transmission	328	AddCRC(pt); // add checksum after data block and initates the transmission
329	}	329	}
330	*/	330	*/
331		331
332	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...	332	void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
333	va_list ap;	333	va_list ap;
334	uint16_t pt = 0;	334	uint16_t pt = 0;
335	uint8_t a, b, c;	335	uint8_t a, b, c;
336	uint8_t ptr = 0;	336	uint8_t ptr = 0;
337		337
338	uint8_t *pdata = 0;	338	uint8_t *pdata = 0;
339	int len = 0;	339	int len = 0;
340		340
341	txd_buffer[pt++] = '#'; // Start character	341	txd_buffer[pt++] = '#'; // Start character
342	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)	342	txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
343	txd_buffer[pt++] = cmd; // Command	343	txd_buffer[pt++] = cmd; // Command
344		344
345	va_start(ap, numofbuffers);	345	va_start(ap, numofbuffers);
346		346
347	if (numofbuffers) {	347	if (numofbuffers) {
348	pdata = va_arg(ap, uint8_t*);	348	pdata = va_arg(ap, uint8_t*);
349	len = va_arg(ap, int);	349	len = va_arg(ap, int);
350	ptr = 0;	350	ptr = 0;
351	numofbuffers--;	351	numofbuffers--;
352	}	352	}
353		353
354	while (len) {	354	while (len) {
355	if (len) {	355	if (len) {
356	a = pdata[ptr++];	356	a = pdata[ptr++];
357	len--;	357	len--;
358	if ((!len) && numofbuffers) {	358	if ((!len) && numofbuffers) {
359	pdata = va_arg(ap, uint8_t*);	359	pdata = va_arg(ap, uint8_t*);
360	len = va_arg(ap, int);	360	len = va_arg(ap, int);
361	ptr = 0;	361	ptr = 0;
362	numofbuffers--;	362	numofbuffers--;
363	}	363	}
364	} else	364	} else
365	a = 0;	365	a = 0;
366	if (len) {	366	if (len) {
367	b = pdata[ptr++];	367	b = pdata[ptr++];
368	len--;	368	len--;
369	if ((!len) && numofbuffers) {	369	if ((!len) && numofbuffers) {
370	pdata = va_arg(ap, uint8_t*);	370	pdata = va_arg(ap, uint8_t*);
371	len = va_arg(ap, int);	371	len = va_arg(ap, int);
372	ptr = 0;	372	ptr = 0;
373	numofbuffers--;	373	numofbuffers--;
374	}	374	}
375	} else	375	} else
376	b = 0;	376	b = 0;
377	if (len) {	377	if (len) {
378	c = pdata[ptr++];	378	c = pdata[ptr++];
379	len--;	379	len--;
380	if ((!len) && numofbuffers) {	380	if ((!len) && numofbuffers) {
381	pdata = va_arg(ap, uint8_t*);	381	pdata = va_arg(ap, uint8_t*);
382	len = va_arg(ap, int);	382	len = va_arg(ap, int);
383	ptr = 0;	383	ptr = 0;
384	numofbuffers--;	384	numofbuffers--;
385	}	385	}
386	} else	386	} else
387	c = 0;	387	c = 0;
388	txd_buffer[pt++] = '=' + (a >> 2);	388	txd_buffer[pt++] = '=' + (a >> 2);
389	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));	389	txd_buffer[pt++] = '=' + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4));
390	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));	390	txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6));
391	txd_buffer[pt++] = '=' + (c & 0x3f);	391	txd_buffer[pt++] = '=' + (c & 0x3f);
392	}	392	}
393	va_end(ap);	393	va_end(ap);
394	AddCRC(pt); // add checksum after data block and initates the transmission	394	AddCRC(pt); // add checksum after data block and initates the transmission
395	}	395	}
396		396
397	// --------------------------------------------------------------------------	397	// --------------------------------------------------------------------------
398	void Decode64(void) {	398	void Decode64(void) {
399	uint8_t a, b, c, d;	399	uint8_t a, b, c, d;
400	uint8_t x, y, z;	400	uint8_t x, y, z;
401	uint8_t ptrIn = 3;	401	uint8_t ptrIn = 3;
402	uint8_t ptrOut = 3;	402	uint8_t ptrOut = 3;
403	uint8_t len = ReceivedBytes - 6;	403	uint8_t len = ReceivedBytes - 6;
404		404
405	while (len) {	405	while (len) {
406	a = rxd_buffer[ptrIn++] - '=';	406	a = rxd_buffer[ptrIn++] - '=';
407	b = rxd_buffer[ptrIn++] - '=';	407	b = rxd_buffer[ptrIn++] - '=';
408	c = rxd_buffer[ptrIn++] - '=';	408	c = rxd_buffer[ptrIn++] - '=';
409	d = rxd_buffer[ptrIn++] - '=';	409	d = rxd_buffer[ptrIn++] - '=';
410	//if(ptrIn > ReceivedBytes - 3) break;	410	//if(ptrIn > ReceivedBytes - 3) break;
411		411
412	x = (a << 2) \| (b >> 4);	412	x = (a << 2) \| (b >> 4);
413	y = ((b & 0x0f) << 4) \| (c >> 2);	413	y = ((b & 0x0f) << 4) \| (c >> 2);
414	z = ((c & 0x03) << 6) \| d;	414	z = ((c & 0x03) << 6) \| d;
415		415
416	if (len--)	416	if (len--)
417	rxd_buffer[ptrOut++] = x;	417	rxd_buffer[ptrOut++] = x;
418	else	418	else
419	break;	419	break;
420	if (len--)	420	if (len--)
421	rxd_buffer[ptrOut++] = y;	421	rxd_buffer[ptrOut++] = y;
422	else	422	else
423	break;	423	break;
424	if (len--)	424	if (len--)
425	rxd_buffer[ptrOut++] = z;	425	rxd_buffer[ptrOut++] = z;
426	else	426	else
427	break;	427	break;
428	}	428	}
429	pRxData = &rxd_buffer[3];	429	pRxData = &rxd_buffer[3];
430	RxDataLen = ptrOut - 3;	430	RxDataLen = ptrOut - 3;
431	}	431	}
432		432
433	// --------------------------------------------------------------------------	433	// --------------------------------------------------------------------------
434	void usart0_ProcessRxData(void) {	434	void usart0_ProcessRxData(void) {
435	// We control the outputTestActive var from here: Count it down.	435	// We control the outputTestActive var from here: Count it down.
436	if (outputTestActive)	436	if (outputTestActive)
437	outputTestActive--;	437	outputTestActive--;
438	// if data in the rxd buffer are not locked immediately return	438	// if data in the rxd buffer are not locked immediately return
439	if (!rxd_buffer_locked)	439	if (!rxd_buffer_locked)
440	return;	440	return;
441	uint8_t tempchar1, tempchar2;	441	uint8_t tempchar1, tempchar2;
442	Decode64(); // decode data block in rxd_buffer	442	Decode64(); // decode data block in rxd_buffer
443		443
444	switch (rxd_buffer[1] - 'a') {	444	switch (rxd_buffer[1] - 'a') {
445		445
446	case FC_ADDRESS:	446	case FC_ADDRESS:
447	switch (rxd_buffer[2]) {	447	switch (rxd_buffer[2]) {
448	#ifdef USE_MK3MAG	448	#ifdef USE_MK3MAG
449	case 'K':// compass value	449	case 'K':// compass value
450	compassHeading = ((Heading_t *)pRxData)->Heading;	450	compassHeading = ((Heading_t *)pRxData)->Heading;
451	// compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;	451	// compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
452	break;	452	break;
453	#endif	453	#endif
454	case 't': // motor test	454	case 't': // motor test
455	if (RxDataLen > 20) {	455	if (RxDataLen > 20) {
456	memcpy(&outputTest[0], (uint8_t*) pRxData, sizeof(outputTest));	456	memcpy(&outputTest[0], (uint8_t*) pRxData, sizeof(outputTest));
457	} else {	457	} else {
458	memcpy(&outputTest[0], (uint8_t*) pRxData, 4);	458	memcpy(&outputTest[0], (uint8_t*) pRxData, 4);
459	}	459	}
460	outputTestActive = 255;	460	outputTestActive = 255;
461	externalControlActive = 255;	461	externalControlActive = 255;
462	break;	462	break;
463		463
464	case 'n':// "Get Mixer Table	464	case 'n':// "Get Mixer Table
465	while (!txd_complete)	465	while (!txd_complete)
466	; // wait for previous frame to be sent	466	; // wait for previous frame to be sent
467	SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));	467	SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
468	break;	468	break;
469		469
470	case 'm':// "Set Mixer Table	470	case 'm':// "Set Mixer Table
471	if (pRxData[0] == EEMIXER_REVISION) {	471	if (pRxData[0] == EEMIXER_REVISION) {
472	memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer));	472	memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer));
473	MixerTable_WriteToEEProm();	473	MixerTable_WriteToEEProm();
474	while (!txd_complete)	474	while (!txd_complete)
475	; // wait for previous frame to be sent	475	; // wait for previous frame to be sent
476	tempchar1 = 1;	476	tempchar1 = 1;
477	} else {	477	} else {
478	tempchar1 = 0;	478	tempchar1 = 0;
479	}	479	}
480	SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1);	480	SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1);
481	break;	481	break;
482		482
483	case 'p': // get PPM channels	483	case 'p': // get PPM channels
484	request_PPMChannels = TRUE;	484	request_PPMChannels = TRUE;
485	break;	485	break;
486		486
487	case 'q':// request settings	487	case 'q':// request settings
488	if (pRxData[0] == 0xFF) {	488	if (pRxData[0] == 0xFF) {
489	pRxData[0] = GetParamByte(PID_ACTIVE_SET);	489	pRxData[0] = GetParamByte(PID_ACTIVE_SET);
490	}	490	}
491	// limit settings range	491	// limit settings range
492	if (pRxData[0] < 1)	492	if (pRxData[0] < 1)
493	pRxData[0] = 1; // limit to 1	493	pRxData[0] = 1; // limit to 1
494	else if (pRxData[0] > 5)	494	else if (pRxData[0] > 5)
495	pRxData[0] = 5; // limit to 5	495	pRxData[0] = 5; // limit to 5
496	// load requested parameter set	496	// load requested parameter set
497	ParamSet_ReadFromEEProm(pRxData[0]);	497	ParamSet_ReadFromEEProm();
498	tempchar1 = pRxData[0];	498	tempchar1 = pRxData[0];
499	tempchar2 = EEPARAM_REVISION;	499	tempchar2 = EEPARAM_REVISION;
500	while (!txd_complete)	500	while (!txd_complete)

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/uart0.c - Rev 1910 → 1927