Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1687 | - | 1 | |
| 2 | /****************************************************************/ |
||
| 3 | /* */ |
||
| 4 | /* NG-Video 5,8GHz */ |
||
| 5 | /* */ |
||
| 6 | /* Copyright (C) 2011 - gebad */ |
||
| 7 | /* */ |
||
| 8 | /* This code is distributed under the GNU Public License */ |
||
| 9 | /* which can be found at http://www.gnu.org/licenses/gpl.txt */ |
||
| 10 | /* */ |
||
| 11 | /****************************************************************/ |
||
| 12 | |||
| 13 | #include <avr/io.h> |
||
| 14 | #include <avr/interrupt.h> |
||
| 15 | #include <string.h> |
||
| 16 | |||
| 17 | #include "usart.h" |
||
| 18 | #include "tracking.h" |
||
| 19 | |||
| 20 | /********************************************************************************************************************/ |
||
| 21 | /* */ |
||
| 22 | /* MK Protokoll */ |
||
| 23 | /* http://www.mikrokopter.de/ucwiki/en/SerialCommands?highlight=(command) */ |
||
| 24 | /* http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) */ |
||
| 25 | /* */ |
||
| 26 | /* Slave-Address Part */ |
||
| 27 | /* 1 FC */ |
||
| 28 | /* 2 NC */ |
||
| 29 | /* 3 MK3MAG */ |
||
| 30 | /* */ |
||
| 31 | /* Auszug von Commands, nur OSD verwendet: */ |
||
| 32 | /* Received by NC | ent by NC */ |
||
| 33 | /* | since NC */ |
||
| 34 | /* Description ID Address Data | ID Address Data Firmware */ |
||
| 35 | /* Debug Request 'd' AnyAddr u8 AutoSendInterval. Value is | 'D' SlaveAddr Debug Data Struct */ |
||
| 36 | /* multiplied by 10 in receiver and , | */ |
||
| 37 | /* then used as milliseconds. Subsciption | */ |
||
| 38 | /* needs to be renewed every 4s. | */ |
||
| 39 | /* Request OSD 'o' NC-Addr 1 byte sending interval (in 10ms steps)| 'O' NC-Addr NaviDataStruct 0.12h */ |
||
| 40 | /* Data | */ |
||
| 41 | /* Redirect UART 'u' NC-Addr 1 byte param for uart selector (0=FC, | - - - 0.12h */ |
||
| 42 | /* 1=MK3MAG, 2=MKGPS), can be switched | */ |
||
| 43 | /* back to NC debug by sending the magic | */ |
||
| 44 | /* packet "0x1B,0x1B,0x55,0xAA,0x00" | */ |
||
| 45 | /* Set 3D-Data 'c' AnyAddr u8 Interval | 'C' NC-Addr struct Data3D 0.14a */ |
||
| 46 | /* Interval */ |
||
| 47 | /* */ |
||
| 48 | /********************************************************************************************************************/ |
||
| 49 | |||
| 50 | char rx_buffer[RXD_BUFFER_SIZE]; |
||
| 51 | |||
| 52 | volatile uint8_t rx_len; |
||
| 53 | volatile uint8_t rx_ready = 0; |
||
| 54 | uint8_t rx_GPS; |
||
| 55 | static char start; |
||
| 56 | static char end; |
||
| 57 | |||
| 58 | /**************************************************************/ |
||
| 59 | /* */ |
||
| 60 | /* USART */ |
||
| 61 | /* 8 Datenbits, 1 Stopbit, keine Parität */ |
||
| 62 | /* */ |
||
| 63 | /**************************************************************/ |
||
| 64 | |||
| 65 | void USART_Init(unsigned int baud) |
||
| 66 | { uint16_t ubrx; |
||
| 67 | |||
| 68 | ubrx = F_CPU/(baud * 16L) - 1; |
||
| 69 | /* Set baud rate */ |
||
| 70 | UBRR0H = (unsigned char)(ubrx>>8); |
||
| 71 | UBRR0L = (unsigned char)ubrx; |
||
| 72 | /* RX Complete Interrupt Enable, Enable receiver and transmitter */ |
||
| 73 | UCSR0B = (1<<RXCIE0) | (1<<RXEN0) | (1<<TXEN0); |
||
| 74 | /* Asynchronous USART, no Parity, Set frame format: 8data, 1stop bit */ |
||
| 75 | UCSR0C = (1<<UCSZ01) | (1<<UCSZ00); // 8data Bit |
||
| 76 | } |
||
| 77 | |||
| 78 | void USART_send_Chr(unsigned char data) |
||
| 79 | { |
||
| 80 | /* Wait for empty transmit Puffer */ |
||
| 81 | while ( !( UCSR0A & (1<<UDRE0)) ) ; |
||
| 82 | /* Put data into Puffer, sends the data */ |
||
| 83 | UDR0 = data; |
||
| 84 | } |
||
| 85 | |||
| 86 | void USART_send_Str(char *str ) |
||
| 87 | { |
||
| 88 | while (*str) { |
||
| 89 | USART_send_Chr(*str); |
||
| 90 | str++; |
||
| 91 | } |
||
| 92 | } |
||
| 93 | |||
| 94 | /* |
||
| 95 | // verwendet Orginal H&I MK-Software http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
||
| 96 | // kann auch vereinfacht werden ==> gleich kodierten String senden char *tx_osd = {"#co?]==EH\r\0"} |
||
| 97 | // Funktion getestet und funktioniert |
||
| 98 | void tx_Mk(unsigned char addr, char cmd, char *data, uint8_t len) |
||
| 99 | { char tx_buffer[TXD_BUFFER_SIZE]; |
||
| 100 | uint8_t tx_i = 0; |
||
| 101 | uint8_t i = 0; |
||
| 102 | unsigned char a,b,c; |
||
| 103 | unsigned int tmpCRC = 0; |
||
| 104 | |||
| 105 | tx_buffer[tx_i++] = '#'; // Start-Byte |
||
| 106 | tx_buffer[tx_i++] = 'a' + addr; // Adress |
||
| 107 | tx_buffer[tx_i++] = cmd; // Command |
||
| 108 | // code64 |
||
| 109 | while (len) { |
||
| 110 | if (len) { a = data[i++]; len--;} else a = 0; |
||
| 111 | if (len) { b = data[i++]; len--;} else b = 0; |
||
| 112 | if (len) { c = data[i++]; len--;} else c = 0; |
||
| 113 | tx_buffer[tx_i++] = '=' + (a >> 2); |
||
| 114 | tx_buffer[tx_i++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
||
| 115 | tx_buffer[tx_i++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
||
| 116 | tx_buffer[tx_i++] = '=' + (c & 0x3f); |
||
| 117 | } |
||
| 118 | // add crc |
||
| 119 | for (i = 0; i < tx_i; i++) { |
||
| 120 | tmpCRC += tx_buffer[i]; |
||
| 121 | } |
||
| 122 | tmpCRC %= 4096; |
||
| 123 | tx_buffer[i++] = '=' + tmpCRC / 64; |
||
| 124 | tx_buffer[i++] = '=' + tmpCRC % 64; |
||
| 125 | tx_buffer[i++] = '\r'; // End-Byte |
||
| 126 | |||
| 127 | tx_buffer[i++] = '\0'; // USART_send_Str(...) ==> End-while |
||
| 128 | USART_send_Str(tx_buffer); |
||
| 129 | } */ |
||
| 130 | |||
| 131 | ISR(USART0_RX_vect) |
||
| 132 | { const char ID[] = ID_LIST; // einfache Erweiterung für andere IDs möglich |
||
| 133 | uint8_t ID_found; |
||
| 134 | char received; |
||
| 135 | static uint8_t line_flag = 1; |
||
| 136 | static char* ptr_write = rx_buffer; |
||
| 137 | static uint8_t frame = 6; |
||
| 138 | |||
| 139 | received = UDR0; |
||
| 140 | if (!rx_ready) { |
||
| 141 | if ((received == start) && line_flag) { // start '#', '$' or 0x80 |
||
| 142 | line_flag = 0; // New line has begun |
||
| 143 | ptr_write = rx_buffer; // Begin at start of buffer |
||
| 144 | rx_len = 0; |
||
| 145 | } |
||
| 146 | if (!line_flag) { // Are we receiving a line? |
||
| 147 | *ptr_write = received; // Add current byte |
||
| 148 | rx_len++; |
||
| 149 | |||
| 150 | // GPS Datensatzende |
||
| 151 | if (rx_GPS) { |
||
| 152 | if (received == end) { // End of MK-GPS or NMEA-line? |
||
| 153 | line_flag = 1; // Yes, start new line |
||
| 154 | if (TRACKING_GPS) { |
||
| 155 | // für Blinken des Antennenzeichens, MK antwortet immer mit cmd Großbuchstaben |
||
| 156 | if ((rx_len > 2) && (rx_buffer[2] >= 'A') && (rx_buffer[2] <= 'Z')) { |
||
| 157 | wi232RX = 1; |
||
| 158 | // nicht benötigte Datensätze bereits jetzt verwerfen |
||
| 159 | for (ID_found=0; ID_found < sizeof(ID) && rx_buffer[2] == ID[ID_found]; ID_found++) |
||
| 160 | rx_ready = ID_found < sizeof(ID); // MK Lock buffer "rx_ready > 0" until line has been processed |
||
| 161 | } |
||
| 162 | } |
||
| 163 | else rx_ready = 1; // NMEA Lock buffer until line has been processed |
||
| 164 | } |
||
| 165 | } |
||
| 166 | // Pololu Micro Serial very reduced/simplified; rx_len-checks 1 higher ==> already incremented |
||
| 167 | else { |
||
| 168 | if ((rx_len == 2) && (received != 0x01)) line_flag = 1; // device |
||
| 169 | if ((rx_len == 3) && (received > 0x01)) frame = 6; else frame = 5; // command |
||
| 170 | if (rx_len == frame) { |
||
| 171 | line_flag = 1; |
||
| 172 | rx_ready = 1; // Lock buffer until line has been processed |
||
| 173 | } |
||
| 174 | } |
||
| 175 | |||
| 176 | ptr_write++; |
||
| 177 | if(rx_len == RXD_BUFFER_SIZE) line_flag = 1; // Line too long? Try again |
||
| 178 | } |
||
| 179 | } |
||
| 180 | } |
||
| 181 | |||
| 182 | // verwendet aus Orginal H&I MK-Software http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
||
| 183 | void Decode64(char *ptrOut, uint8_t len, uint8_t ptrIn) |
||
| 184 | { unsigned char a,b,c,d; |
||
| 185 | unsigned char ptr = 0; |
||
| 186 | unsigned char x,y,z; |
||
| 187 | |||
| 188 | while(len) |
||
| 189 | { |
||
| 190 | a = rx_buffer[ptrIn++] - '='; |
||
| 191 | b = rx_buffer[ptrIn++] - '='; |
||
| 192 | c = rx_buffer[ptrIn++] - '='; |
||
| 193 | d = rx_buffer[ptrIn++] - '='; |
||
| 194 | |||
| 195 | x = (a << 2) | (b >> 4); |
||
| 196 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
| 197 | z = ((c & 0x03) << 6) | d; |
||
| 198 | if(len--) ptrOut[ptr++] = x; else break; |
||
| 199 | if(len--) ptrOut[ptr++] = y; else break; |
||
| 200 | if(len--) ptrOut[ptr++] = z; else break; |
||
| 201 | } |
||
| 202 | } |
||
| 203 | |||
| 204 | // eingearbeitet Original H&I MK-Software mkprotocol.h und mkprotocol.c |
||
| 205 | char rx_line_decode(void) |
||
| 206 | { uint8_t ret = 0; |
||
| 207 | uint8_t crc1,crc2; |
||
| 208 | int tmpCRC = 0; |
||
| 209 | |||
| 210 | // rx_ready gleichzeitig usart rx_buffer schreiben gesperrt |
||
| 211 | if (rx_ready) { |
||
| 212 | // Checksumme |
||
| 213 | // verwendet http://www.mikrokopter.de/ucwiki/en/SerialProtocol?highlight=(protocol) |
||
| 214 | for(uint8_t i = 0; i < rx_len - 3;i++) { |
||
| 215 | tmpCRC += rx_buffer[i]; |
||
| 216 | } |
||
| 217 | tmpCRC %= 4096; |
||
| 218 | crc1 = '=' + tmpCRC / 64; |
||
| 219 | crc2 = '=' + tmpCRC % 64; |
||
| 220 | |||
| 221 | if ((crc1 == rx_buffer[rx_len-3]) && (crc2 == rx_buffer[rx_len-2])) { |
||
| 222 | Decode64(data_decode, rx_len - 6, 3); // Daten ohne Satzzeichen, header und crc |
||
| 223 | ret = rx_buffer[2]; |
||
| 224 | cli(); |
||
| 225 | rx_timeout = 0; // wenn kein gültiger Datensatz rx_ID ==> Counter wird nicht mehr rückgesetzt |
||
| 226 | sei(); |
||
| 227 | } |
||
| 228 | } |
||
| 229 | // Puffer Schreiben entsperren, neuer MK-Datensatz kann wieder empfangen/gepuffert werden |
||
| 230 | rx_ready = 0; |
||
| 231 | return(ret); |
||
| 232 | } |
||
| 233 | |||
| 234 | uint8_t hexDigitToInt(uint8_t digit) |
||
| 235 | { |
||
| 236 | if (digit >= '0' && digit <= '9') return digit - '0'; |
||
| 237 | if (digit >= 'a' && digit <= 'f') return digit - 'a' + 10; |
||
| 238 | if (digit >= 'A' && digit <= 'F') return digit - 'A' + 10; |
||
| 239 | return 0; |
||
| 240 | } |
||
| 241 | |||
| 242 | uint8_t decodeNMEA(void) |
||
| 243 | { uint8_t ret = 0; |
||
| 244 | uint8_t crc; |
||
| 245 | uint8_t tmpCRC = 0; |
||
| 246 | uint8_t i; |
||
| 247 | |||
| 248 | if (rx_ready == 1 && rx_len > 0) { |
||
| 249 | // Calculate checksum |
||
| 250 | for (i = 1; i < rx_len && rx_buffer[i] != '*'; i++) { |
||
| 251 | tmpCRC ^= rx_buffer[i]; |
||
| 252 | } |
||
| 253 | if (rx_len >= i + 3) { |
||
| 254 | crc = hexDigitToInt(rx_buffer[i + 1]) << 4 | hexDigitToInt(rx_buffer[i + 2]); |
||
| 255 | if (crc == tmpCRC) { |
||
| 256 | rx_buffer[i] = 0; |
||
| 257 | strcpy(data_decode, &rx_buffer[1]); // Data without $, crc |
||
| 258 | ret = 1; |
||
| 259 | wi232RX = 1; // So antenna-symbol will blink |
||
| 260 | cli(); |
||
| 261 | rx_timeout = 0; // Got valid data, reset counter |
||
| 262 | sei(); |
||
| 263 | } |
||
| 264 | } |
||
| 265 | } |
||
| 266 | if (rx_timeout < RX_TIME_OLD) wi232RX = 1; |
||
| 267 | rx_ready = 0; // Unlock buffer, next NMEA string can be received |
||
| 268 | return ret; |
||
| 269 | } |
||
| 270 | |||
| 271 | void USART_RX_Mode(uint8_t tracking) |
||
| 272 | { |
||
| 273 | switch (tracking) { |
||
| 274 | case TRACKING_GPS: |
||
| 275 | start = '#'; |
||
| 276 | end = '\r'; |
||
| 277 | rx_GPS = 1; |
||
| 278 | break; |
||
| 279 | case TRACKING_NMEA: |
||
| 280 | start = '$'; |
||
| 281 | end = '\n'; |
||
| 282 | rx_GPS = 1; |
||
| 283 | break; |
||
| 284 | default: |
||
| 285 | start = 0x80; |
||
| 286 | rx_GPS = 0; |
||
| 287 | break; |
||
| 288 | } |
||
| 289 | } |
||
| 290 | |||
| 291 | uint8_t Get_Pololu_cmd(char *ptrOut, uint8_t ptrIn) |
||
| 292 | { |
||
| 293 | rx_len -=2; // ohne Start-Char und device |
||
| 294 | if (rx_ready == 1) { |
||
| 295 | for (uint8_t i = 0; i < rx_len; i++) |
||
| 296 | ptrOut[i] = rx_buffer[ptrIn++]; // ab Pololu-Command |
||
| 297 | } |
||
| 298 | else |
||
| 299 | rx_len = 0; |
||
| 300 | rx_ready = 0; // Puffer Schreiben entsperren, neuer Pololu-Datensatz kann wieder empfangen/gepuffert werden |
||
| 301 | return(rx_len); |
||
| 302 | } |