WebSVN - FlightCtrl - Diff - Rev 2099 and 2102 - /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"
#include "commands.h"
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
 
#define FALSE   0
#define TRUE    1
 
int8_t displayBuff[DISPLAYBUFFSIZE];
uint8_t dispPtr = 0;
 
uint8_t requestedDebugLabel = 255;
uint8_t request_verInfo = FALSE;
uint8_t request_externalControl = FALSE;
uint8_t request_debugData = FALSE;
uint8_t request_data3D = FALSE;
uint8_t request_PPMChannels = FALSE;
uint8_t request_servoTest = 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;
 
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 servoTestActive = 0;
uint8_t servoTest[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
                "Gyro P          ", //0
                "Gyro R          ",
                "Gyro Y          ",
                "Attitude P      ",
                "Attitude R      ",
                "Attitude Y      ", //5
                "Target P        ",
                "Target R        ",
                "Target Y        ",
                "Error P         ",
                "Error R         ", //10
                "Error Y         ",
        "Term P          ",
                "Term R          ",
                "Term Y          ",
                "FlightMode      ", //15
                "RC P            ",
                "RC Thr          ",
        "ServoFinal P    ",
                "ServoFinal T    ",
                "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 servoTestActive var from here: Count it down.
        if (servoTestActive)
          servoTestActive--;
        // 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(&servoTest[0], (uint8_t*) pRxData, sizeof(servoTest));
                        } else {
                                memcpy(&servoTest[0], (uint8_t*) pRxData, 4);
                        }
                        servoTestActive = 255;
                        externalControlActive = 255;
                        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 (!isMotorRunning) // 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 (!isMotorRunning) // save settings only if motors are off
                        {
                                if ((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(1);
                                        configuration_paramSetDidChange();
                                        tempchar[0] = 1;
                                        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 '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 (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/10)); // convert to multiple of 0.1 deg
                data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg
                data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/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;
        }
 
 
        if (request_servoTest && txd_complete) {
                sendOutData('T', FC_ADDRESS, 0);
                request_servoTest = 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 = 0;
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg
          // TODO: To 0..359 interval.
          data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg
          sendOutData('O', FC_ADDRESS, 3, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
          OSD_timer = setDelay(OSD_interval);
          request_OSD = FALSE;
        }
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/uart0.c @ 2088 - Rev 2099 → 2102