WebSVN - FlightCtrl - Diff - Rev 2095 and 2097 - /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 "menu.h"
#include "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.h"
#include "commands.h"
 
#ifdef USE_DIRECT_GPS
#include "mk3mag.h"
#endif
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE   0
#define TRUE    1
 
uint8_t requestedDebugLabel = 255;
 
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_PPMChannels = FALSE;
uint8_t request_motorTest = FALSE;
uint8_t request_variables = FALSE;
uint8_t request_OSD = FALSE;
 
/*
#define request_verInfo         (1<<0)
#define request_externalControl (1<<1)
#define request_display         (1<<3)
#define request_display1        (1<<4)
#define request_debugData       (1<<5)
#define request_data3D          (1<<6)
#define request_PPMChannels     (1<<7)
#define request_motorTest       (1<<8)
#define request_variables       (1<<9)
#define request_OSD             (1<<10)
*/
 
//uint16_t request = 0;
 
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 motorTestActive = 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;
 
Data3D_t data3D;
 
uint16_t debugData_timer;
uint16_t data3D_timer;
uint16_t OSD_timer;
uint16_t debugData_interval = 0; // in 1ms
uint16_t data3D_interval = 0; // in 1ms
uint16_t OSD_interval = 0;
 
#ifdef USE_DIRECT_GPS
int16_t toMk3MagTimer;
#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       ",
                "GyroRoll        ",
                "GyroYaw         ", //5
                "PitchTerm       ",
                "RollTerm        ",
                "ThrottleTerm    ",
                "YawTerm         ",
                "heightP         ", //10
                "heightI         ",
        "heightD         ",
                "gyroActivity    ",
                "ca              ",
                "GActivityDivider", //15
                "NaviMode        ",
                "NaviStatus      ",
        "NaviStickP      ",
                "NaviStickR      ",
                "control act wghd", //20
                "acc vector wghd ",
                "Height[dm]      ",
                "dHeight         ",
                "acc vector      ",
                "EFT             ", //25
                "naviPitch       ",
                "naviRoll        ",
                "tolerance       ",
                "Gyro Act Cont.  ",
                "GPS altitude    ", //30
                "GPS vert accura "
  };
 
/****************************************************************/
/*              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_DIRECT_GPS
        toMk3MagTimer = setDelay(220);
#endif
 
        versionInfo.SWMajor = VERSION_MAJOR;
        versionInfo.SWMinor = VERSION_MINOR;
        versionInfo.SWPatch = VERSION_PATCH;
        versionInfo.protoMajor = VERSION_SERIAL_MAJOR;
        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 checksum;
        static uint8_t ptr_rxd_buffer = 0;
        uint8_t checksum1, checksum2;
        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
                checksum = c; // init checksum
        }
        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
                        checksum += c; // update checksum
                } else { // termination character was received
                        // the last 2 bytes are no subject for checksum calculation
                        // they are the checksum itself
                        checksum -= rxd_buffer[ptr_rxd_buffer - 2];
                        checksum -= rxd_buffer[ptr_rxd_buffer - 1];
                        // calculate checksum from transmitted data
                        checksum %= 4096;
                        checksum1 = '=' + checksum / 64;
                        checksum2 = '=' + checksum % 64;
                        // compare checksum to transmitted checksum bytes
                        if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2
                                        == 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 addChecksum(uint16_t datalen) {
        uint16_t tmpchecksum = 0, i;
        for (i = 0; i < datalen; i++) {
                tmpchecksum += txd_buffer[i];
        }
        tmpchecksum %= 4096;
        txd_buffer[i++] = '=' + (tmpchecksum >> 6);
        txd_buffer[i++] = '=' + (tmpchecksum & 0x3F);
        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);
 Addchecksum(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);
        addChecksum(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 motorTestActive var from here: Count it down.
        if (motorTestActive)
                motorTestActive--;
        // if data in the rxd buffer are not locked immediately return
        if (!rxd_buffer_locked)
                return;
        uint8_t tempchar[3];
        decode64(); // decode data block in rxd_buffer
 
        switch (rxd_buffer[1] - 'a') {
 
        case FC_ADDRESS:
                switch (rxd_buffer[2]) {
#ifdef USE_DIRECT_GPS
                case 'K':// compass value
                  // What is the point of this - the compass will overwrite this soon?
                magneticHeading = ((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);
                        }
                        motorTestActive = 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 *) &mixerMatrix, sizeof(mixerMatrix));
                        break;
 
                case 'm':// "Set Mixer Table
                        if (pRxData[0] == EEMIXER_REVISION) {
                                memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix));
                                mixerMatrix_writeToEEProm();
                                while (!txd_complete)
                                        ; // wait for previous frame to be sent
                                tempchar[0] = 1;
                        } else {
                                tempchar[0] = 0;
                        }
                        sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
                        break;
 
                case 'p': // get PPM channels
                        request_PPMChannels = TRUE;
                        break;
 
        case 'i':// Read IMU configuration
            tempchar[0] = IMUCONFIG_REVISION;
            tempchar[1] = sizeof(IMUConfig);
            while (!txd_complete)
                ; // wait for previous frame to be sent
            sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
            break;
 
        case 'j':// Save IMU configuration
          if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
          {
              if ((pRxData[0] == IMUCONFIG_REVISION) && (pRxData[1] == sizeof(IMUConfig))) {
                  memcpy(&IMUConfig, (uint8_t*) &pRxData[2], sizeof(IMUConfig));
                  IMUConfig_writeToEEprom();
                  tempchar[0] = 1; //indicate ok data
              } else {
                  tempchar[0] = 0; //indicate bad data
              }
              while (!txd_complete)
                  ; // wait for previous frame to be sent
              sendOutData('J', FC_ADDRESS, 1, &tempchar, 1);
          }
          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]);
 
                        tempchar[0] = pRxData[0];
                        tempchar[1] = EEPARAM_REVISION;
                        tempchar[2] = sizeof(staticParams);
                        while (!txd_complete)
                                ; // wait for previous frame to be sent
                        sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (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) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings
                                {
                                        memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams));
                                        paramSet_writeToEEProm(pRxData[0]);
                                        setActiveParamSet(pRxData[0]);
                                        configuration_paramSetDidChange();
                                        tempchar[0] = getActiveParamSet();
                                        beepNumber(tempchar[0]);
                                } else {
                                        tempchar[0] = 0; //indicate bad data
                                }
                                while (!txd_complete)
                                        ; // wait for previous frame to be sent
                                sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
                        }
                        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
                        requestedDebugLabel = pRxData[0];
                        if (requestedDebugLabel > 31)
                                requestedDebugLabel = 31;
                        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
                        remoteKeys |= pRxData[0];
                        if (remoteKeys)
                                displayLine = 0;
                        request_display = TRUE;
                        break;
 
                case 'l':// request for display columns
                        menuItem = pRxData[0];
                        request_display1 = TRUE;
                        break;
 
                case 'o':// request for OSD data (FC style)
                  OSD_interval = (uint16_t) pRxData[0] * 10;
                  if (OSD_interval > 0)
                    request_OSD = 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 *) &versionInfo, sizeof(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 (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
                uint8_t label[16]; // local sram buffer
                memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
                sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
                                sizeof(requestedDebugLabel), label, 16);
                requestedDebugLabel = 0xFF;
        }
 
        if (confirmFrame && txd_complete) { // Datensatz ohne checksum 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.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
                data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
                data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
                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_DIRECT_GPS
        if((checkDelay(toMk3MagTimer)) && txd_complete) {
                toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
                toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // 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;
                toMk3MagTimer = 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;
        }
 
        if (request_variables && txd_complete) {
                sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
                request_variables = FALSE;
        }
 
        if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
          int32_t height = analog_getHeight();
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
          data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
          sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
          OSD_timer = setDelay(OSD_interval);
          request_OSD = FALSE;
        }
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c - Rev 2095 → 2097