0,0 → 1,302 |
|
/****************************************************************/ |
/* */ |
/* NG-Video 5,8GHz */ |
/* */ |
/* Copyright (C) 2011 - gebad */ |
/* */ |
/* This code is distributed under the GNU Public License */ |
/* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
/* */ |
/****************************************************************/ |
|
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <string.h> |
|
#include "usart.h" |
#include "tracking.h" |
|
/********************************************************************************************************************/ |
/* */ |
/* MK Protokoll */ |
/* http://www.mikrokopter.de/ucwiki/en/SerialCommands?highlight=(command) */ |
/* http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) */ |
/* */ |
/* Slave-Address Part */ |
/* 1 FC */ |
/* 2 NC */ |
/* 3 MK3MAG */ |
/* */ |
/* Auszug von Commands, nur OSD verwendet: */ |
/* Received by NC | ent by NC */ |
/* | since NC */ |
/* Description ID Address Data | ID Address Data Firmware */ |
/* Debug Request 'd' AnyAddr u8 AutoSendInterval. Value is | 'D' SlaveAddr Debug Data Struct */ |
/* multiplied by 10 in receiver and , | */ |
/* then used as milliseconds. Subsciption | */ |
/* needs to be renewed every 4s. | */ |
/* Request OSD 'o' NC-Addr 1 byte sending interval (in 10ms steps)| 'O' NC-Addr NaviDataStruct 0.12h */ |
/* Data | */ |
/* Redirect UART 'u' NC-Addr 1 byte param for uart selector (0=FC, | - - - 0.12h */ |
/* 1=MK3MAG, 2=MKGPS), can be switched | */ |
/* back to NC debug by sending the magic | */ |
/* packet "0x1B,0x1B,0x55,0xAA,0x00" | */ |
/* Set 3D-Data 'c' AnyAddr u8 Interval | 'C' NC-Addr struct Data3D 0.14a */ |
/* Interval */ |
/* */ |
/********************************************************************************************************************/ |
|
char rx_buffer[RXD_BUFFER_SIZE]; |
|
volatile uint8_t rx_len; |
volatile uint8_t rx_ready = 0; |
uint8_t rx_GPS; |
static char start; |
static char end; |
|
/**************************************************************/ |
/* */ |
/* USART */ |
/* 8 Datenbits, 1 Stopbit, keine Parität */ |
/* */ |
/**************************************************************/ |
|
void USART_Init(unsigned int baud) |
{ uint16_t ubrx; |
|
ubrx = F_CPU/(baud * 16L) - 1; |
/* Set baud rate */ |
UBRR0H = (unsigned char)(ubrx>>8); |
UBRR0L = (unsigned char)ubrx; |
/* RX Complete Interrupt Enable, Enable receiver and transmitter */ |
UCSR0B = (1<<RXCIE0) | (1<<RXEN0) | (1<<TXEN0); |
/* Asynchronous USART, no Parity, Set frame format: 8data, 1stop bit */ |
UCSR0C = (1<<UCSZ01) | (1<<UCSZ00); // 8data Bit |
} |
|
void USART_send_Chr(unsigned char data) |
{ |
/* Wait for empty transmit Puffer */ |
while ( !( UCSR0A & (1<<UDRE0)) ) ; |
/* Put data into Puffer, sends the data */ |
UDR0 = data; |
} |
|
void USART_send_Str(char *str ) |
{ |
while (*str) { |
USART_send_Chr(*str); |
str++; |
} |
} |
|
/* |
// verwendet Orginal H&I MK-Software http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
// kann auch vereinfacht werden ==> gleich kodierten String senden char *tx_osd = {"#co?]==EH\r\0"} |
// Funktion getestet und funktioniert |
void tx_Mk(unsigned char addr, char cmd, char *data, uint8_t len) |
{ char tx_buffer[TXD_BUFFER_SIZE]; |
uint8_t tx_i = 0; |
uint8_t i = 0; |
unsigned char a,b,c; |
unsigned int tmpCRC = 0; |
|
tx_buffer[tx_i++] = '#'; // Start-Byte |
tx_buffer[tx_i++] = 'a' + addr; // Adress |
tx_buffer[tx_i++] = cmd; // Command |
// code64 |
while (len) { |
if (len) { a = data[i++]; len--;} else a = 0; |
if (len) { b = data[i++]; len--;} else b = 0; |
if (len) { c = data[i++]; len--;} else c = 0; |
tx_buffer[tx_i++] = '=' + (a >> 2); |
tx_buffer[tx_i++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
tx_buffer[tx_i++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
tx_buffer[tx_i++] = '=' + (c & 0x3f); |
} |
// add crc |
for (i = 0; i < tx_i; i++) { |
tmpCRC += tx_buffer[i]; |
} |
tmpCRC %= 4096; |
tx_buffer[i++] = '=' + tmpCRC / 64; |
tx_buffer[i++] = '=' + tmpCRC % 64; |
tx_buffer[i++] = '\r'; // End-Byte |
|
tx_buffer[i++] = '\0'; // USART_send_Str(...) ==> End-while |
USART_send_Str(tx_buffer); |
} */ |
|
ISR(USART0_RX_vect) |
{ const char ID[] = ID_LIST; // einfache Erweiterung für andere IDs möglich |
uint8_t ID_found; |
char received; |
static uint8_t line_flag = 1; |
static char* ptr_write = rx_buffer; |
static uint8_t frame = 6; |
|
received = UDR0; |
if (!rx_ready) { |
if ((received == start) && line_flag) { // start '#', '$' or 0x80 |
line_flag = 0; // New line has begun |
ptr_write = rx_buffer; // Begin at start of buffer |
rx_len = 0; |
} |
if (!line_flag) { // Are we receiving a line? |
*ptr_write = received; // Add current byte |
rx_len++; |
|
// GPS Datensatzende |
if (rx_GPS) { |
if (received == end) { // End of MK-GPS or NMEA-line? |
line_flag = 1; // Yes, start new line |
if (TRACKING_GPS) { |
// für Blinken des Antennenzeichens, MK antwortet immer mit cmd Großbuchstaben |
if ((rx_len > 2) && (rx_buffer[2] >= 'A') && (rx_buffer[2] <= 'Z')) { |
wi232RX = 1; |
// nicht benötigte Datensätze bereits jetzt verwerfen |
for (ID_found=0; ID_found < sizeof(ID) && rx_buffer[2] == ID[ID_found]; ID_found++) |
rx_ready = ID_found < sizeof(ID); // MK Lock buffer "rx_ready > 0" until line has been processed |
} |
} |
else rx_ready = 1; // NMEA Lock buffer until line has been processed |
} |
} |
// Pololu Micro Serial very reduced/simplified; rx_len-checks 1 higher ==> already incremented |
else { |
if ((rx_len == 2) && (received != 0x01)) line_flag = 1; // device |
if ((rx_len == 3) && (received > 0x01)) frame = 6; else frame = 5; // command |
if (rx_len == frame) { |
line_flag = 1; |
rx_ready = 1; // Lock buffer until line has been processed |
} |
} |
|
ptr_write++; |
if(rx_len == RXD_BUFFER_SIZE) line_flag = 1; // Line too long? Try again |
} |
} |
} |
|
// verwendet aus Orginal H&I MK-Software http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
void Decode64(char *ptrOut, uint8_t len, uint8_t ptrIn) |
{ unsigned char a,b,c,d; |
unsigned char ptr = 0; |
unsigned char x,y,z; |
|
while(len) |
{ |
a = rx_buffer[ptrIn++] - '='; |
b = rx_buffer[ptrIn++] - '='; |
c = rx_buffer[ptrIn++] - '='; |
d = rx_buffer[ptrIn++] - '='; |
|
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if(len--) ptrOut[ptr++] = x; else break; |
if(len--) ptrOut[ptr++] = y; else break; |
if(len--) ptrOut[ptr++] = z; else break; |
} |
} |
|
// eingearbeitet Original H&I MK-Software mkprotocol.h und mkprotocol.c |
char rx_line_decode(void) |
{ uint8_t ret = 0; |
uint8_t crc1,crc2; |
int tmpCRC = 0; |
|
// rx_ready gleichzeitig usart rx_buffer schreiben gesperrt |
if (rx_ready) { |
// Checksumme |
// verwendet http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
for(uint8_t i = 0; i < rx_len - 3;i++) { |
tmpCRC += rx_buffer[i]; |
} |
tmpCRC %= 4096; |
crc1 = '=' + tmpCRC / 64; |
crc2 = '=' + tmpCRC % 64; |
|
if ((crc1 == rx_buffer[rx_len-3]) && (crc2 == rx_buffer[rx_len-2])) { |
Decode64(data_decode, rx_len - 6, 3); // Daten ohne Satzzeichen, header und crc |
ret = rx_buffer[2]; |
cli(); |
rx_timeout = 0; // wenn kein gültiger Datensatz rx_ID ==> Counter wird nicht mehr rückgesetzt |
sei(); |
} |
} |
// Puffer Schreiben entsperren, neuer MK-Datensatz kann wieder empfangen/gepuffert werden |
rx_ready = 0; |
return(ret); |
} |
|
uint8_t hexDigitToInt(uint8_t digit) |
{ |
if (digit >= '0' && digit <= '9') return digit - '0'; |
if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10; |
if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10; |
return 0; |
} |
|
uint8_t decodeNMEA(void) |
{ uint8_t ret = 0; |
uint8_t crc; |
uint8_t tmpCRC = 0; |
uint8_t i; |
|
if (rx_ready == 1 && rx_len > 0) { |
// Calculate checksum |
for (i = 1; i < rx_len && rx_buffer[i] != '*'; i++) { |
tmpCRC ^= rx_buffer[i]; |
} |
if (rx_len >= i + 3) { |
crc = hexDigitToInt(rx_buffer[i + 1]) << 4 | hexDigitToInt(rx_buffer[i + 2]); |
if (crc == tmpCRC) { |
rx_buffer[i] = 0; |
strcpy(data_decode, &rx_buffer[1]); // Data without $, crc |
ret = 1; |
wi232RX = 1; // So antenna-symbol will blink |
cli(); |
rx_timeout = 0; // Got valid data, reset counter |
sei(); |
} |
} |
} |
if (rx_timeout < RX_TIME_OLD) wi232RX = 1; |
rx_ready = 0; // Unlock buffer, next NMEA string can be received |
return ret; |
} |
|
void USART_RX_Mode(uint8_t tracking) |
{ |
switch (tracking) { |
case TRACKING_GPS: |
start = '#'; |
end = '\r'; |
rx_GPS = 1; |
break; |
case TRACKING_NMEA: |
start = '$'; |
end = '\n'; |
rx_GPS = 1; |
break; |
default: |
start = 0x80; |
rx_GPS = 0; |
break; |
} |
} |
|
uint8_t Get_Pololu_cmd(char *ptrOut, uint8_t ptrIn) |
{ |
rx_len -=2; // ohne Start-Char und device |
if (rx_ready == 1) { |
for (uint8_t i = 0; i < rx_len; i++) |
ptrOut[i] = rx_buffer[ptrIn++]; // ab Pololu-Command |
} |
else |
rx_len = 0; |
rx_ready = 0; // Puffer Schreiben entsperren, neuer Pololu-Datensatz kann wieder empfangen/gepuffert werden |
return(rx_len); |
} |