Subversion Repositories FlightCtrl

Rev

Rev 909 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 909 Rev 916
1
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
3
// + only for non-profit use
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
 
7
 
8
#include <avr/io.h>
8
#include <avr/io.h>
9
#include <avr/interrupt.h>
9
#include <avr/interrupt.h>
10
#include <avr/wdt.h>
10
#include <avr/wdt.h>
11
 
11
 
12
#include "eeprom.h"
12
#include "eeprom.h"
13
#include "main.h"
13
#include "main.h"
14
#include "menu.h"
14
#include "menu.h"
15
#include "timer0.h"
15
#include "timer0.h"
16
#include "uart.h"
16
#include "uart.h"
17
#include "fc.h"
17
#include "fc.h"
18
#include "_Settings.h"
18
#include "_Settings.h"
19
#include "rc.h"
19
#include "rc.h"
20
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
20
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
21
#include "ubx.h"
21
#include "ubx.h"
22
#endif
22
#endif
23
#ifdef USE_MK3MAG
23
#ifdef USE_MK3MAG
24
#include "mk3mag.h"
24
#include "mk3mag.h"
25
#endif
25
#endif
26
 
26
 
27
 
27
 
28
 
28
 
29
#define FALSE   0
29
#define FALSE   0
30
#define TRUE    1
30
#define TRUE    1
31
 
31
 
32
//int8_t test __attribute__ ((section (".noinit")));
32
//int8_t test __attribute__ ((section (".noinit")));
33
uint8_t RequestVerInfo                  = FALSE;
33
uint8_t RequestVerInfo                  = FALSE;
34
uint8_t RequestExternalControl  = FALSE;
34
uint8_t RequestExternalControl  = FALSE;
35
uint8_t RequestDisplay                  = FALSE;
35
uint8_t RequestDisplay                  = FALSE;
36
uint8_t RequestDebugData                = FALSE;
36
uint8_t RequestDebugData                = FALSE;
37
uint8_t RequestDebugLabel               = 255;
37
uint8_t RequestDebugLabel               = 255;
38
uint8_t RequestChannelOnly              = FALSE;
38
uint8_t RequestChannelOnly              = FALSE;
39
uint8_t RemotePollDisplayLine   = 0;
39
uint8_t RemotePollDisplayLine   = 0;
40
 
40
 
41
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
41
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
42
volatile uint8_t rxd_buffer_locked = FALSE;
42
volatile uint8_t rxd_buffer_locked = FALSE;
43
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
43
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
44
volatile uint8_t txd_complete = TRUE;
44
volatile uint8_t txd_complete = TRUE;
45
volatile uint8_t ReceivedBytes = 0;
45
volatile uint8_t ReceivedBytes = 0;
46
 
46
 
47
 
47
 
48
uint8_t PcAccess = 100;
48
uint8_t PcAccess = 100;
49
uint8_t MotorTest[4] = {0,0,0,0};
49
uint8_t MotorTest[4] = {0,0,0,0};
50
uint8_t DubWiseKeys[4] = {0,0,0,0};
50
uint8_t DubWiseKeys[4] = {0,0,0,0};
51
uint8_t MySlaveAddr = 0;
51
uint8_t MySlaveAddr = 0;
52
uint8_t ConfirmFrame;
52
uint8_t ConfirmFrame;
53
 
53
 
54
DebugOut_t              DebugOut;
54
DebugOut_t              DebugOut;
55
ExternControl_t ExternControl;
55
ExternControl_t ExternControl;
56
VersionInfo_t   VersionInfo;
56
VersionInfo_t   VersionInfo;
57
 
57
 
58
int16_t  Debug_Timer;
58
int16_t  Debug_Timer;
59
 
59
 
60
#ifdef USE_MK3MAG
60
#ifdef USE_MK3MAG
61
int16_t Compass_Timer;
61
int16_t Compass_Timer;
62
#endif
62
#endif
63
 
63
 
64
 
64
 
65
const uint8_t ANALOG_LABEL[32][16] =
65
const uint8_t ANALOG_LABEL[32][16] =
66
{
66
{
67
   //1234567890123456
67
   //1234567890123456
68
    "IntegralPitch   ", //0
68
    "IntegralNick    ", //0
69
    "IntegralRoll    ",
69
    "IntegralRoll    ",
70
    "AccPitch        ",
70
    "AccNick         ",
71
    "AccRoll         ",
71
    "AccRoll         ",
72
    "GyroYaw         ",
72
    "GyroYaw         ",
73
    "ReadingHeight   ", //5
73
    "ReadingHeight   ", //5
74
    "AccZ            ",
74
    "AccZ            ",
75
    "Thrust          ",
75
    "Gas             ",
76
    "Receiver Level  ",
76
    "Receiver Level  ",
77
    "Voltage         ",
77
    "Voltage         ",
78
#ifdef HEXAKOPTER
78
#ifdef HEXAKOPTER
79
    "Motor FrontLeft ", //10
79
    "Motor FrontLeft ", //10
80
    "Motor FrontRight",
80
    "Motor FrontRight",
81
    "Motor RearLeft  ",
81
    "Motor RearLeft  ",
82
    "Motor RearRight ",
82
    "Motor RearRight ",
83
    "Motor Right     ",
83
    "Motor Right     ",
84
    "Motor Left      ", //15
84
    "Motor Left      ", //15
85
#else
85
#else
86
    "Motor FrontLeft ", //10
86
    "Motor FrontLeft ", //10
87
    "Motor FrontRight",
87
    "Motor FrontRight",
88
    "Motor_RearLeft  ",
88
    "Motor_RearLeft  ",
89
    "Motor_RearRight ",
89
    "Motor_RearRight ",
90
    "Motor_Right     ",
90
    "Motor_Right     ",
91
    "Motor_Left      ", //15
91
    "Motor_Left      ", //15
92
#endif
92
#endif
93
    "Acc_Z           ",
93
    "Acc_Z           ",
94
    "SPI Error       ",
94
    "SPI Error       ",
95
    "SPI Ok          ",
95
    "SPI Ok          ",
96
    "                ",
96
    "                ",
97
    "Servo           ", //20
97
    "Servo           ", //20
98
    "Pitch           ",
98
    "Nick            ",
99
    "Roll            ",
99
    "Roll            ",
100
    "                ",
100
    "                ",
101
    "                ",
101
    "                ",
102
    "                ", //25
102
    "                ", //25
103
    "                ",
103
    "                ",
104
    "                ",
104
    "                ",
105
    "                ",
105
    "                ",
106
    "                ",
106
    "                ",
107
    "GPS_Pitch       ", //30
107
    "GPS_Nick        ", //30
108
    "GPS_Roll        "
108
    "GPS_Roll        "
109
};
109
};
110
 
110
 
111
 
111
 
112
 
112
 
113
/****************************************************************/
113
/****************************************************************/
114
/*              Initialization of the USART0                    */
114
/*              Initialization of the USART0                    */
115
/****************************************************************/
115
/****************************************************************/
116
void USART0_Init (void)
116
void USART0_Init (void)
117
{
117
{
118
        uint8_t sreg = SREG;
118
        uint8_t sreg = SREG;
119
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
119
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
120
 
120
 
121
        // disable all interrupts before configuration
121
        // disable all interrupts before configuration
122
        cli();
122
        cli();
123
 
123
 
124
        // disable RX-Interrupt
124
        // disable RX-Interrupt
125
        UCSR0B &= ~(1 << RXCIE0);
125
        UCSR0B &= ~(1 << RXCIE0);
126
        // disable TX-Interrupt
126
        // disable TX-Interrupt
127
        UCSR0B &= ~(1 << TXCIE0);
127
        UCSR0B &= ~(1 << TXCIE0);
128
 
128
 
129
        // set direction of RXD0 and TXD0 pins
129
        // set direction of RXD0 and TXD0 pins
130
        // set RXD0 (PD0) as an input pin
130
        // set RXD0 (PD0) as an input pin
131
        PORTD |= (1 << PORTD0);
131
        PORTD |= (1 << PORTD0);
132
        DDRD &= ~(1 << DDD0);
132
        DDRD &= ~(1 << DDD0);
133
        // set TXD0 (PD1) as an output pin
133
        // set TXD0 (PD1) as an output pin
134
        PORTD |= (1 << PORTD1);
134
        PORTD |= (1 << PORTD1);
135
        DDRD |=  (1 << DDD1);
135
        DDRD |=  (1 << DDD1);
136
 
136
 
137
        // USART0 Baud Rate Register
137
        // USART0 Baud Rate Register
138
        // set clock divider
138
        // set clock divider
139
        UBRR0H = (uint8_t)(ubrr >> 8);
139
        UBRR0H = (uint8_t)(ubrr >> 8);
140
        UBRR0L = (uint8_t)ubrr;
140
        UBRR0L = (uint8_t)ubrr;
141
 
141
 
142
        // USART0 Control and Status Register A, B, C
142
        // USART0 Control and Status Register A, B, C
143
 
143
 
144
        // enable double speed operation in
144
        // enable double speed operation in
145
        UCSR0A |= (1 << U2X0);
145
        UCSR0A |= (1 << U2X0);
146
        // enable receiver and transmitter in
146
        // enable receiver and transmitter in
147
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
147
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
148
        // set asynchronous mode
148
        // set asynchronous mode
149
        UCSR0C &= ~(1 << UMSEL01);
149
        UCSR0C &= ~(1 << UMSEL01);
150
        UCSR0C &= ~(1 << UMSEL00);
150
        UCSR0C &= ~(1 << UMSEL00);
151
        // no parity
151
        // no parity
152
        UCSR0C &= ~(1 << UPM01);
152
        UCSR0C &= ~(1 << UPM01);
153
        UCSR0C &= ~(1 << UPM00);
153
        UCSR0C &= ~(1 << UPM00);
154
        // 1 stop bit
154
        // 1 stop bit
155
        UCSR0C &= ~(1 << USBS0);
155
        UCSR0C &= ~(1 << USBS0);
156
        // 8-bit
156
        // 8-bit
157
        UCSR0B &= ~(1 << UCSZ02);
157
        UCSR0B &= ~(1 << UCSZ02);
158
        UCSR0C |=  (1 << UCSZ01);
158
        UCSR0C |=  (1 << UCSZ01);
159
        UCSR0C |=  (1 << UCSZ00);
159
        UCSR0C |=  (1 << UCSZ00);
160
 
160
 
161
                // flush receive buffer
161
                // flush receive buffer
162
        while ( UCSR0A & (1<<RXC0) ) UDR0;
162
        while ( UCSR0A & (1<<RXC0) ) UDR0;
163
 
163
 
164
        // enable interrupts at the end
164
        // enable interrupts at the end
165
        // enable RX-Interrupt
165
        // enable RX-Interrupt
166
        UCSR0B |= (1 << RXCIE0);
166
        UCSR0B |= (1 << RXCIE0);
167
        // enable TX-Interrupt
167
        // enable TX-Interrupt
168
        UCSR0B |= (1 << TXCIE0);
168
        UCSR0B |= (1 << TXCIE0);
169
 
169
 
170
        rxd_buffer_locked = FALSE;
170
        rxd_buffer_locked = FALSE;
171
        txd_complete = TRUE;
171
        txd_complete = TRUE;
172
 
172
 
173
        Debug_Timer = SetDelay(200);
173
        Debug_Timer = SetDelay(200);
174
 
174
 
175
        #ifdef USE_MK3MAG
175
        #ifdef USE_MK3MAG
176
        Compass_Timer = SetDelay(220);
176
        Compass_Timer = SetDelay(220);
177
        #endif
177
        #endif
178
 
178
 
179
        // restore global interrupt flags
179
        // restore global interrupt flags
180
    SREG = sreg;
180
    SREG = sreg;
181
}
181
}
182
 
182
 
183
/****************************************************************/
183
/****************************************************************/
184
/*               USART0 transmitter ISR                         */
184
/*               USART0 transmitter ISR                         */
185
/****************************************************************/
185
/****************************************************************/
186
ISR(USART0_TX_vect)
186
ISR(USART0_TX_vect)
187
{
187
{
188
        static uint16_t ptr_txd_buffer = 0;
188
        static uint16_t ptr_txd_buffer = 0;
189
        uint8_t tmp_tx;
189
        uint8_t tmp_tx;
190
        if(!txd_complete) // transmission not completed
190
        if(!txd_complete) // transmission not completed
191
        {
191
        {
192
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
192
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
193
                tmp_tx = txd_buffer[ptr_txd_buffer];
193
                tmp_tx = txd_buffer[ptr_txd_buffer];
194
                // if terminating character or end of txd buffer was reached
194
                // if terminating character or end of txd buffer was reached
195
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
195
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
196
                {
196
                {
197
                        ptr_txd_buffer = 0; // reset txd pointer
197
                        ptr_txd_buffer = 0; // reset txd pointer
198
                        txd_complete = 1; // stop transmission
198
                        txd_complete = 1; // stop transmission
199
                }
199
                }
200
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
200
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
201
        }
201
        }
202
        // transmission completed
202
        // transmission completed
203
        else ptr_txd_buffer = 0;
203
        else ptr_txd_buffer = 0;
204
}
204
}
205
 
205
 
206
/****************************************************************/
206
/****************************************************************/
207
/*               USART0 receiver ISR                            */
207
/*               USART0 receiver ISR                            */
208
/****************************************************************/
208
/****************************************************************/
209
ISR(USART0_RX_vect)
209
ISR(USART0_RX_vect)
210
{
210
{
211
        static uint16_t crc;
211
        static uint16_t crc;
212
        static uint8_t ptr_rxd_buffer = 0;
212
        static uint8_t ptr_rxd_buffer = 0;
213
        uint8_t crc1, crc2;
213
        uint8_t crc1, crc2;
214
        uint8_t c;
214
        uint8_t c;
215
 
215
 
216
        c = UDR0;  // catch the received byte
216
        c = UDR0;  // catch the received byte
217
 
217
 
218
        #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
218
        #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
219
        // If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart.
219
        // If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart.
220
        // The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected.
220
        // The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected.
221
        #if defined (__AVR_ATmega644P__)
221
        #if defined (__AVR_ATmega644P__)
222
        if(BoardRelease == 10) ubx_parser(c);
222
        if(BoardRelease == 10) ubx_parser(c);
223
        #else
223
        #else
224
        ubx_parser(c);
224
        ubx_parser(c);
225
        #endif
225
        #endif
226
        #endif
226
        #endif
227
 
227
 
228
        if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
228
        if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
229
 
229
 
230
        // the rxd buffer is unlocked
230
        // the rxd buffer is unlocked
231
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
231
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
232
        {
232
        {
233
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
233
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
234
                crc = c; // init crc
234
                crc = c; // init crc
235
        }
235
        }
236
        #if 0
236
        #if 0
237
        else if (ptr_rxd_buffer == 1) // handle address
237
        else if (ptr_rxd_buffer == 1) // handle address
238
        {
238
        {
239
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
239
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
240
                crc += c; // update crc
240
                crc += c; // update crc
241
        }
241
        }
242
        #endif
242
        #endif
243
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
243
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
244
        {
244
        {
245
                if(c != '\r') // no termination character
245
                if(c != '\r') // no termination character
246
                {
246
                {
247
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
247
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
248
                        crc += c; // update crc
248
                        crc += c; // update crc
249
                }
249
                }
250
                else // termination character was received
250
                else // termination character was received
251
                {
251
                {
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
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
254
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
255
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
255
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
256
                        // calculate checksum from transmitted data
256
                        // calculate checksum from transmitted data
257
                        crc %= 4096;
257
                        crc %= 4096;
258
                        crc1 = '=' + crc / 64;
258
                        crc1 = '=' + crc / 64;
259
                        crc2 = '=' + crc % 64;
259
                        crc2 = '=' + crc % 64;
260
                        // compare checksum to transmitted checksum bytes
260
                        // compare checksum to transmitted checksum bytes
261
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
261
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
262
                        {   // checksum valid
262
                        {   // checksum valid
263
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
263
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
264
                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
264
                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
265
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
265
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
266
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
266
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
267
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
267
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
268
                        }
268
                        }
269
                        else
269
                        else
270
                        {       // checksum invalid
270
                        {       // checksum invalid
271
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
271
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
272
                        }
272
                        }
273
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
273
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
274
                }
274
                }
275
        }
275
        }
276
        else // rxd buffer overrun
276
        else // rxd buffer overrun
277
        {
277
        {
278
                ptr_rxd_buffer = 0; // reset rxd buffer
278
                ptr_rxd_buffer = 0; // reset rxd buffer
279
                rxd_buffer_locked = FALSE; // unlock rxd buffer
279
                rxd_buffer_locked = FALSE; // unlock rxd buffer
280
        }
280
        }
281
 
281
 
282
}
282
}
283
 
283
 
284
 
284
 
285
// --------------------------------------------------------------------------
285
// --------------------------------------------------------------------------
286
void AddCRC(uint16_t datalen)
286
void AddCRC(uint16_t datalen)
287
{
287
{
288
        uint16_t tmpCRC = 0, i;
288
        uint16_t tmpCRC = 0, i;
289
        for(i = 0; i < datalen; i++)
289
        for(i = 0; i < datalen; i++)
290
        {
290
        {
291
                tmpCRC += txd_buffer[i];
291
                tmpCRC += txd_buffer[i];
292
        }
292
        }
293
        tmpCRC %= 4096;
293
        tmpCRC %= 4096;
294
        txd_buffer[i++] = '=' + tmpCRC / 64;
294
        txd_buffer[i++] = '=' + tmpCRC / 64;
295
        txd_buffer[i++] = '=' + tmpCRC % 64;
295
        txd_buffer[i++] = '=' + tmpCRC % 64;
296
        txd_buffer[i++] = '\r';
296
        txd_buffer[i++] = '\r';
297
        txd_complete = FALSE;
297
        txd_complete = FALSE;
298
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
298
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
299
}
299
}
300
 
300
 
301
 
301
 
302
 
302
 
303
// --------------------------------------------------------------------------
303
// --------------------------------------------------------------------------
304
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
304
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
305
{
305
{
306
        uint16_t pt = 0;
306
        uint16_t pt = 0;
307
        uint8_t a,b,c;
307
        uint8_t a,b,c;
308
        uint8_t ptr = 0;
308
        uint8_t ptr = 0;
309
 
309
 
310
        txd_buffer[pt++] = '#';         // Start character
310
        txd_buffer[pt++] = '#';         // Start character
311
        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
311
        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
312
        txd_buffer[pt++] = cmd;         // Command
312
        txd_buffer[pt++] = cmd;         // Command
313
 
313
 
314
        while(len)
314
        while(len)
315
        {
315
        {
316
                if(len) { a = snd[ptr++]; len--;} else a = 0;
316
                if(len) { a = snd[ptr++]; len--;} else a = 0;
317
                if(len) { b = snd[ptr++]; len--;} else b = 0;
317
                if(len) { b = snd[ptr++]; len--;} else b = 0;
318
                if(len) { c = snd[ptr++]; len--;} else c = 0;
318
                if(len) { c = snd[ptr++]; len--;} else c = 0;
319
                txd_buffer[pt++] = '=' + (a >> 2);
319
                txd_buffer[pt++] = '=' + (a >> 2);
320
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
320
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
321
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
321
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
322
                txd_buffer[pt++] = '=' + ( c & 0x3f);
322
                txd_buffer[pt++] = '=' + ( c & 0x3f);
323
        }
323
        }
324
        AddCRC(pt); // add checksum after data block and initates the transmission
324
        AddCRC(pt); // add checksum after data block and initates the transmission
325
}
325
}
326
 
326
 
327
 
327
 
328
// --------------------------------------------------------------------------
328
// --------------------------------------------------------------------------
329
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
329
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
330
{
330
{
331
        uint8_t a,b,c,d;
331
        uint8_t a,b,c,d;
332
        uint8_t ptr = 0;
332
        uint8_t ptr = 0;
333
        uint8_t x,y,z;
333
        uint8_t x,y,z;
334
        while(len)
334
        while(len)
335
        {
335
        {
336
                a = rxd_buffer[ptrIn++] - '=';
336
                a = rxd_buffer[ptrIn++] - '=';
337
                b = rxd_buffer[ptrIn++] - '=';
337
                b = rxd_buffer[ptrIn++] - '=';
338
                c = rxd_buffer[ptrIn++] - '=';
338
                c = rxd_buffer[ptrIn++] - '=';
339
                d = rxd_buffer[ptrIn++] - '=';
339
                d = rxd_buffer[ptrIn++] - '=';
340
                if(ptrIn > max - 2) break;
340
                if(ptrIn > max - 2) break;
341
 
341
 
342
                x = (a << 2) | (b >> 4);
342
                x = (a << 2) | (b >> 4);
343
                y = ((b & 0x0f) << 4) | (c >> 2);
343
                y = ((b & 0x0f) << 4) | (c >> 2);
344
                z = ((c & 0x03) << 6) | d;
344
                z = ((c & 0x03) << 6) | d;
345
 
345
 
346
                if(len--) ptrOut[ptr++] = x; else break;
346
                if(len--) ptrOut[ptr++] = x; else break;
347
                if(len--) ptrOut[ptr++] = y; else break;
347
                if(len--) ptrOut[ptr++] = y; else break;
348
                if(len--) ptrOut[ptr++] = z; else break;
348
                if(len--) ptrOut[ptr++] = z; else break;
349
        }
349
        }
350
}
350
}
351
 
351
 
352
 
352
 
353
// --------------------------------------------------------------------------
353
// --------------------------------------------------------------------------
354
void USART0_ProcessRxData(void)
354
void USART0_ProcessRxData(void)
355
{
355
{
356
        // if data in the rxd buffer are not locked immediately return
356
        // if data in the rxd buffer are not locked immediately return
357
        if(!rxd_buffer_locked) return;
357
        if(!rxd_buffer_locked) return;
358
 
358
 
359
        #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
359
        #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
360
        uint16_t tmp_int_arr1[1]; // local int buffer
360
        uint16_t tmp_int_arr1[1]; // local int buffer
361
        #endif
361
        #endif
362
        uint8_t tmp_char_arr2[2]; // local char buffer
362
        uint8_t tmp_char_arr2[2]; // local char buffer
363
 
363
 
364
 
364
 
365
        switch(rxd_buffer[2])
365
        switch(rxd_buffer[2])
366
        {
366
        {
367
                #ifdef USE_MK3MAG
367
                #ifdef USE_MK3MAG
368
                case 'K':// Compass value
368
                case 'K':// Compass value
369
                        Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes);
369
                        Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes);
370
                        CompassHeading = tmp_int_arr1[0];
370
                        CompassHeading = tmp_int_arr1[0];
371
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
371
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
372
                        break;
372
                        break;
373
                #endif
373
                #endif
374
                case 'a':// Labels of the Analog Debug outputs
374
                case 'a':// Labels of the Analog Debug outputs
375
                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
375
                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
376
                        RequestDebugLabel = tmp_char_arr2[0];
376
                        RequestDebugLabel = tmp_char_arr2[0];
377
                        PcAccess = 255;
377
                        PcAccess = 255;
378
                        break;
378
                        break;
379
                case 'b': // extern control
379
                case 'b': // extern control
380
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
380
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
381
                        RemoteButtons |= ExternControl.RemoteButtons;
381
                        RemoteButtons |= ExternControl.RemoteButtons;
382
                        ConfirmFrame = ExternControl.Frame;
382
                        ConfirmFrame = ExternControl.Frame;
383
                        break;
383
                        break;
384
                case 'c': // extern control with debug request
384
                case 'c': // extern control with debug request
385
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
385
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
386
                        RemoteButtons |= ExternControl.RemoteButtons;
386
                        RemoteButtons |= ExternControl.RemoteButtons;
387
                        ConfirmFrame = ExternControl.Frame;
387
                        ConfirmFrame = ExternControl.Frame;
388
                        RequestDebugData = TRUE;
388
                        RequestDebugData = TRUE;
389
                        PcAccess = 255;
389
                        PcAccess = 255;
390
                        break;
390
                        break;
391
                case 'h':// x-1 display columns
391
                case 'h':// x-1 display columns
392
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
392
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
393
                        RemoteButtons |= tmp_char_arr2[0];
393
                        RemoteButtons |= tmp_char_arr2[0];
394
                        if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE;
394
                        if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE;
395
                        else RequestChannelOnly = FALSE; // keine Displaydaten
395
                        else RequestChannelOnly = FALSE; // keine Displaydaten
396
                        RequestDisplay = TRUE;
396
                        RequestDisplay = TRUE;
397
                        break;
397
                        break;
398
                case 't':// motor test
398
                case 't':// motor test
399
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
399
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
400
                        PcAccess = 255;
400
                        PcAccess = 255;
401
                        break;
401
                        break;
402
                case 'k':// keys from DubWise
402
                case 'k':// keys from DubWise
403
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
403
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
404
                        ConfirmFrame = DubWiseKeys[3];
404
                        ConfirmFrame = DubWiseKeys[3];
405
                        PcAccess = 255;
405
                        PcAccess = 255;
406
                        break;
406
                        break;
407
                case 'v': // get version and board release
407
                case 'v': // get version and board release
408
                        RequestVerInfo = TRUE;
408
                        RequestVerInfo = TRUE;
409
                        break;
409
                        break;
410
                case 'g':// get external control data
410
                case 'g':// get external control data
411
                        RequestExternalControl = TRUE;
411
                        RequestExternalControl = TRUE;
412
                        break;
412
                        break;
413
                case 'q':// get settings
413
                case 'q':// get settings
414
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
414
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
415
                        while(!txd_complete);
415
                        while(!txd_complete);
416
                        if(tmp_char_arr2[0] != 0xff)
416
                        if(tmp_char_arr2[0] != 0xff)
417
                        {
417
                        {
418
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
418
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
419
                                // load requested parameter set
419
                                // load requested parameter set
420
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
420
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
421
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
421
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
422
                        }
422
                        }
423
                        else // send active parameter set
423
                        else // send active parameter set
424
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
424
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
425
 
425
 
426
                        break;
426
                        break;
427
 
427
 
428
                case 'l':
428
                case 'l':
429
                case 'm':
429
                case 'm':
430
                case 'n':
430
                case 'n':
431
                case 'o':
431
                case 'o':
432
                case 'p': // save parameterset
432
                case 'p': // save parameterset
433
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
433
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
434
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
434
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
435
                        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
435
                        TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
436
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
436
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
437
                        Beep(GetActiveParamSet());
437
                        Beep(GetActiveParamSet());
438
                        break;
438
                        break;
439
 
439
 
440
 
440
 
441
        }
441
        }
442
        // unlock the rxd buffer after processing
442
        // unlock the rxd buffer after processing
443
        rxd_buffer_locked = FALSE;
443
        rxd_buffer_locked = FALSE;
444
}
444
}
445
 
445
 
446
//############################################################################
446
//############################################################################
447
//Routine für die Serielle Ausgabe
447
//Routine für die Serielle Ausgabe
448
int16_t uart_putchar (int8_t c)
448
int16_t uart_putchar (int8_t c)
449
//############################################################################
449
//############################################################################
450
{
450
{
451
        if (c == '\n')
451
        if (c == '\n')
452
                uart_putchar('\r');
452
                uart_putchar('\r');
453
        // wait until previous character was send
453
        // wait until previous character was send
454
        loop_until_bit_is_set(UCSR0A, UDRE0);
454
        loop_until_bit_is_set(UCSR0A, UDRE0);
455
        //Ausgabe des Zeichens
455
        //Ausgabe des Zeichens
456
        UDR0 = c;
456
        UDR0 = c;
457
        return (0);
457
        return (0);
458
}
458
}
459
 
459
 
460
 
460
 
461
//---------------------------------------------------------------------------------------------
461
//---------------------------------------------------------------------------------------------
462
void USART0_TransmitTxData(void)
462
void USART0_TransmitTxData(void)
463
{
463
{
464
        if(!txd_complete) return;
464
        if(!txd_complete) return;
465
 
465
 
466
        if(RequestExternalControl && txd_complete)            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
466
        if(RequestExternalControl && txd_complete)            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
467
        {
467
        {
468
                SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
468
                SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
469
                RequestExternalControl = FALSE;
469
                RequestExternalControl = FALSE;
470
        }
470
        }
471
 
471
 
472
        #ifdef USE_MK3MAG
472
        #ifdef USE_MK3MAG
473
    if((CheckDelay(Compass_Timer)) && txd_complete)
473
    if((CheckDelay(Compass_Timer)) && txd_complete)
474
        {
474
        {
475
                ToMk3Mag.Attitude[0] = (int16_t) (IntegralPitch / 108);  // approx. 0,1 Deg
475
                ToMk3Mag.Attitude[0] = (int16_t) (IntegralNick / 108);  // approx. 0,1 Deg
476
                ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108);   // approx. 0,1 Deg
476
                ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108);   // approx. 0,1 Deg
477
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
477
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
478
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
478
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
479
                ToMk3Mag.CalState = CompassCalState;
479
                ToMk3Mag.CalState = CompassCalState;
480
                SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
480
                SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
481
                // the last state is 5 and should be send only once to avoid multiple flash writing
481
                // the last state is 5 and should be send only once to avoid multiple flash writing
482
                if(CompassCalState > 4)  CompassCalState = 0;
482
                if(CompassCalState > 4)  CompassCalState = 0;
483
                Compass_Timer = SetDelay(99);
483
                Compass_Timer = SetDelay(99);
484
        }
484
        }
485
        #endif
485
        #endif
486
 
486
 
487
    if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete)
487
    if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete)
488
    {
488
    {
489
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
489
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
490
          RequestDebugData = FALSE;
490
          RequestDebugData = FALSE;
491
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
491
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
492
    }
492
    }
493
 
493
 
494
    if(RequestDebugLabel != 255) // Texte für die Analogdaten
494
    if(RequestDebugLabel != 255) // Texte für die Analogdaten
495
     {
495
     {
496
      SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16);
496
      SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16);
497
      RequestDebugLabel = 255;
497
      RequestDebugLabel = 255;
498
         }
498
         }
499
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
499
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
500
         {
500
         {
501
      txd_buffer[0] = '#';
501
      txd_buffer[0] = '#';
502
      txd_buffer[1] = ConfirmFrame;
502
      txd_buffer[1] = ConfirmFrame;
503
      txd_buffer[2] = '\r';
503
      txd_buffer[2] = '\r';
504
      txd_complete = 0;
504
      txd_complete = 0;
505
      ConfirmFrame = 0;
505
      ConfirmFrame = 0;
506
      UDR0 = txd_buffer[0];
506
      UDR0 = txd_buffer[0];
507
     }
507
     }
508
     if(RequestDisplay && txd_complete)
508
     if(RequestDisplay && txd_complete)
509
         {
509
         {
510
      LCD_PrintMenu();
510
      LCD_PrintMenu();
511
          RequestDisplay = FALSE;
511
          RequestDisplay = FALSE;
512
      if(++RemotePollDisplayLine == 4 || RequestChannelOnly)
512
      if(++RemotePollDisplayLine == 4 || RequestChannelOnly)
513
      {
513
      {
514
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
514
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
515
       RemotePollDisplayLine = -1;
515
       RemotePollDisplayLine = -1;
516
      }
516
      }
517
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
517
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
518
         }
518
         }
519
    if(RequestVerInfo && txd_complete)
519
    if(RequestVerInfo && txd_complete)
520
     {
520
     {
521
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
521
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
522
          RequestVerInfo = FALSE;
522
          RequestVerInfo = FALSE;
523
     }
523
     }
524
 
524
 
525
}
525
}
526
 
526
 
527
 
527