Subversion Repositories FlightCtrl

Rev

Rev 2164 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2164 Rev 2189
Line 4... Line 4...
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>
Line 7... Line 7...
7
 
7
 
8
#include "eeprom.h"
-
 
9
#include "menu.h"
8
#include "eeprom.h"
10
#include "timer0.h"
9
#include "timer0.h"
11
#include "uart0.h"
10
#include "uart0.h"
12
#include "rc.h"
11
#include "rc.h"
13
#include "externalControl.h"
12
#include "externalControl.h"
14
#include "output.h"
13
#include "debug.h"
15
#include "attitude.h"
14
#include "profiler.h"
Line 16... Line 15...
16
#include "commands.h"
15
#include "beeper.h"
17
 
16
 
18
#ifdef USE_DIRECT_GPS
17
#ifdef USE_DIRECT_GPS
Line 24... Line 23...
24
#define MK3MAG_ADDRESS 3
23
#define MK3MAG_ADDRESS 3
Line 25... Line 24...
25
 
24
 
26
#define FALSE   0
25
#define FALSE   0
Line 27... Line 26...
27
#define TRUE    1
26
#define TRUE    1
Line -... Line 27...
-
 
27
 
-
 
28
DebugOut_t debugOut;
-
 
29
 
28
 
30
uint8_t requestedAnalogLabel = 255;
29
uint8_t requestedDebugLabel = 255;
31
uint8_t requestedProfilerLabel = 255;
30
 
32
 
31
uint8_t request_verInfo = FALSE;
33
uint8_t request_verInfo;
32
uint8_t request_externalControl = FALSE;
34
uint8_t request_externalControl;
33
uint8_t request_display = FALSE;
35
uint8_t request_display;
34
uint8_t request_display1 = FALSE;
36
uint8_t request_display1;
35
uint8_t request_debugData = FALSE;
37
uint8_t request_debugData;
36
uint8_t request_data3D = FALSE;
38
uint8_t request_profilerData;
37
uint8_t request_PPMChannels = FALSE;
39
uint8_t request_PPMChannels;
-
 
40
uint8_t request_outputTest;
Line 38... Line 41...
38
uint8_t request_motorTest = FALSE;
41
uint8_t request_variables;
39
uint8_t request_variables = FALSE;
42
uint8_t request_OSD;
40
uint8_t request_OSD = FALSE;
43
uint8_t request_DCM_matrix;
41
 
44
 
Line 52... Line 55...
52
#define request_OSD             (1<<10)
55
#define request_OSD             (1<<10)
53
*/
56
*/
Line 54... Line 57...
54
 
57
 
Line 55... Line -...
55
//uint16_t request = 0;
-
 
56
 
-
 
57
uint8_t displayLine = 0;
58
//uint16_t request = 0;
58
 
59
 
59
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
60
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
60
volatile uint8_t rxd_buffer_locked = FALSE;
61
volatile uint8_t rxd_buffer_locked = FALSE;
61
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
62
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
62
volatile uint8_t txd_complete = TRUE;
63
volatile uint8_t txd_complete;
63
volatile uint8_t receivedBytes = 0;
64
volatile uint8_t receivedBytes;
Line 64... Line 65...
64
volatile uint8_t *pRxData = 0;
65
volatile uint8_t *pRxData;
65
volatile uint8_t rxDataLen = 0;
66
volatile uint8_t rxDataLen;
66
 
67
 
Line 67... Line 68...
67
uint8_t motorTestActive = 0;
68
uint8_t outputTestActive;
68
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
69
uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
69
uint8_t confirmFrame;
70
uint8_t confirmFrame;
Line 70... Line 71...
70
 
71
 
Line 71... Line 72...
71
typedef struct {
72
typedef struct {
72
        int16_t heading;
73
        int16_t heading;
73
}__attribute__((packed)) Heading_t;
74
}__attribute__((packed)) Heading_t;
74
 
75
 
-
 
76
IMUData imuData;
75
Data3D_t data3D;
77
 
76
 
78
uint16_t debugDataTimer;
Line 77... Line 79...
77
uint16_t debugData_timer;
79
uint16_t profilerDataTimer;
78
uint16_t data3D_timer;
80
uint16_t OSDDataTimer;
79
uint16_t OSD_timer;
81
uint16_t debugDataInterval; // in 1ms
Line 80... Line 82...
80
uint16_t debugData_interval = 0; // in 1ms
82
uint16_t profilerDataInterval; // in 1ms
81
uint16_t data3D_interval = 0; // in 1ms
-
 
82
uint16_t OSD_interval = 0;
83
uint16_t imuDataInterval; // in 1ms
-
 
84
uint16_t OSDDataInterval;
-
 
85
 
-
 
86
#ifdef USE_DIRECT_GPS
-
 
87
int16_t toMk3MagTimer;
-
 
88
#endif
-
 
89
 
-
 
90
// keep lables in flash to save 512 bytes of sram space
-
 
91
           //1234567890123456
-
 
92
const char analogLabel0[]  PROGMEM = "AngleRoll";
-
 
93
const char analogLabel1[]  PROGMEM = "AnglePitch";
-
 
94
const char analogLabel2[]  PROGMEM = "AngleYaw";
-
 
95
const char analogLabel3[]  PROGMEM = "GyroX(Roll)";
-
 
96
const char analogLabel4[]  PROGMEM = "GyroY(Pitch)";
-
 
97
const char analogLabel5[]  PROGMEM = "GyroZ(Yaw)";
-
 
98
const char analogLabel6[]  PROGMEM = "AccX(0.01m/s^2)";
-
 
99
const char analogLabel7[]  PROGMEM = "AccY(0.01m/s^2)";
-
 
100
const char analogLabel8[]  PROGMEM = "AccZ(0.01m/s^2)";
-
 
101
const char analogLabel9[]  PROGMEM = "RC pitch";
-
 
102
const char analogLabel10[] PROGMEM = "RC yaw";
-
 
103
const char analogLabel11[] PROGMEM = "RC throttle";
-
 
104
const char analogLabel12[] PROGMEM = "Roll";
-
 
105
const char analogLabel13[] PROGMEM = "Pitch";
-
 
106
const char analogLabel14[] PROGMEM = "rollControl";
-
 
107
const char analogLabel15[] PROGMEM = "pitchControl";
-
 
108
const char analogLabel16[] PROGMEM = "M1";
-
 
109
const char analogLabel17[] PROGMEM = "M2";
-
 
110
const char analogLabel18[] PROGMEM = "M3";
-
 
111
const char analogLabel19[] PROGMEM = "M4";
-
 
112
const char analogLabel20[] PROGMEM = "flightmode";
-
 
113
const char analogLabel21[] PROGMEM = "Att freq";
-
 
114
const char analogLabel22[] PROGMEM = "Height[dm]";
-
 
115
const char analogLabel23[] PROGMEM = "dHeight";
-
 
116
const char analogLabel24[] PROGMEM = "attitudeSumCount";
83
 
117
const char analogLabel25[] PROGMEM = "simpleAirPressure";
-
 
118
const char analogLabel26[] PROGMEM = "OCR0A";
84
#ifdef USE_DIRECT_GPS
119
const char analogLabel27[] PROGMEM = "filteredAirPressure";
85
int16_t toMk3MagTimer;
120
const char analogLabel28[] PROGMEM = "height";
86
#endif
121
const char analogLabel29[] PROGMEM = "Gyro Act Cont.";
87
 
122
const char analogLabel30[] PROGMEM = "GPS altitude";
88
// keep lables in flash to save 512 bytes of sram space
123
const char analogLabel31[] PROGMEM = "GPS vert accura";
89
const prog_uint8_t ANALOG_LABEL[32][16] = {
124
 
90
                //1234567890123456
125
PGM_P ANALOG_LABELS[] PROGMEM = {
91
                "AnglePitch      ", //0
126
    analogLabel0,
92
                "AngleRoll       ",
127
    analogLabel1,
93
                "AngleYaw        ",
128
    analogLabel2,
94
                "GyroPitch       ",
129
    analogLabel3,
95
                "GyroRoll        ",
130
    analogLabel4,
96
                "GyroYaw         ", //5
131
    analogLabel5,
97
                "AccPitch(0.1deg)",
132
    analogLabel6,
98
                "AccRoll(0.1deg) ",
133
    analogLabel7,
99
                "AccZ            ",
134
    analogLabel8,
100
                "RC pitch        ",
135
    analogLabel9,
101
                "RC roll         ", //10
136
    analogLabel10,
102
                "                ",
137
    analogLabel11,
103
        "zerothOrderCorr ",
138
    analogLabel12,
104
                "DriftCompPitch  ",
139
    analogLabel13,
105
                "DriftCompRoll   ",
140
    analogLabel14,
106
                "GActivityDivider", //15
141
    analogLabel15,
107
                "AccPitch        ",
142
    analogLabel16,
108
                "AccRoll         ",
143
    analogLabel17,
109
        "CorrectionSum pi",
144
    analogLabel18,
110
                "CorrectionSum ro",
145
    analogLabel19,
111
                "control act wghd", //20
146
    analogLabel20,
112
                "acc vector wghd ",
147
    analogLabel21,
113
                "Height[dm]      ",
148
    analogLabel22,
114
                "dHeight         ",
149
    analogLabel23,
115
                "acc vector      ",
150
    analogLabel24,
Line 116... Line 151...
116
                "EFT             ", //25
151
    analogLabel25,
117
                "naviPitch       ",
152
    analogLabel26,
118
                "naviRoll        ",
153
    analogLabel27,
119
                "Rate Tolerance  ",
154
    analogLabel28,
120
                "Gyro Act Cont.  ",
155
    analogLabel29,
121
                "GPS altitude    ", //30
156
    analogLabel30,
Line 122... Line 157...
122
                "GPS vert accura "
157
    analogLabel31
123
  };
158
};
Line 124... Line 159...
124
 
159
 
Line 178... Line 213...
178
        UCSR0B |= (1 << RXCIE0);
213
        UCSR0B |= (1 << RXCIE0);
179
        // enable TX-Interrupt
214
        // enable TX-Interrupt
180
        UCSR0B |= (1 << TXCIE0);
215
        UCSR0B |= (1 << TXCIE0);
Line 181... Line 216...
181
 
216
 
182
        // initialize the debug timer
217
        // initialize the debug timer
-
 
218
        debugDataTimer = setDelay(debugDataInterval);
Line 183... Line 219...
183
        debugData_timer = setDelay(debugData_interval);
219
        profilerDataTimer = setDelay(profilerDataInterval);
184
 
220
 
185
        // unlock rxd_buffer
221
        // unlock rxd_buffer
186
        rxd_buffer_locked = FALSE;
222
        rxd_buffer_locked = FALSE;
Line 238... Line 274...
238
 
274
 
239
        if (rxd_buffer_locked)
275
        if (rxd_buffer_locked)
Line 240... Line 276...
240
                return; // if rxd buffer is locked immediately return
276
                return; // if rxd buffer is locked immediately return
241
 
277
 
242
        // the rxd buffer is unlocked
278
        // the rxd buffer is unlocked
243
        if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
279
        if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and synchronization character is received
244
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
280
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
245
                checksum = c; // init checksum
281
                checksum = c; // init checksum
246
        }
282
        }
247
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
283
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incoming bytes
248
                if (c != '\r') { // no termination character
284
                if (c != '\r') { // no termination character
249
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
285
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
250
                        checksum += c; // update checksum
286
                        checksum += c; // update checksum
Line 293... Line 329...
293
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
329
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
294
}
330
}
Line 295... Line 331...
295
 
331
 
296
// --------------------------------------------------------------------------
332
// --------------------------------------------------------------------------
297
// application example:
333
// application example:
298
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
334
// sendData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
299
/*
335
/*
300
 void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
336
 void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
301
 va_list ap;
337
 va_list ap;
302
 uint16_t txd_bufferIndex = 0;
338
 uint16_t txd_bufferIndex = 0;
303
 uint8_t *currentBuffer;
339
 uint8_t *currentBuffer;
304
 uint8_t currentBufferIndex;
340
 uint8_t currentBufferIndex;
Line 342... Line 378...
342
 va_end(ap);
378
 va_end(ap);
343
 Addchecksum(pt); // add checksum after data block and initates the transmission
379
 Addchecksum(pt); // add checksum after data block and initates the transmission
344
 }
380
 }
345
 */
381
 */
Line 346... Line 382...
346
 
382
 
347
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
383
void sendData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
348
        va_list ap;
384
        va_list ap;
349
        uint16_t pt = 0;
385
        uint16_t pt = 0;
350
        uint8_t a, b, c;
386
        uint8_t a, b, c;
Line 445... Line 481...
445
        rxDataLen = ptrOut - 3;
481
        rxDataLen = ptrOut - 3;
446
}
482
}
Line 447... Line 483...
447
 
483
 
448
// --------------------------------------------------------------------------
484
// --------------------------------------------------------------------------
449
void usart0_processRxData(void) {
485
void usart0_processRxData(void) {
450
        // We control the motorTestActive var from here: Count it down.
486
        // We control the outputTestActive var from here: Count it down.
451
        if (motorTestActive)
487
        if (outputTestActive)
452
                motorTestActive--;
488
          outputTestActive--;
453
        // if data in the rxd buffer are not locked immediately return
489
        // if data in the rxd buffer are not locked immediately return
454
        if (!rxd_buffer_locked)
490
        if (!rxd_buffer_locked)
-
 
491
                return;
455
                return;
492
 
456
        uint8_t tempchar[3];
493
        uint8_t tempchar[3];
Line 457... Line 494...
457
        decode64(); // decode data block in rxd_buffer
494
        decode64(); // decode data block in rxd_buffer
Line 466... Line 503...
466
                magneticHeading = ((Heading_t *)pRxData)->heading;
503
                magneticHeading = ((Heading_t *)pRxData)->heading;
467
                // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
504
                // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
468
                break;
505
                break;
469
#endif
506
#endif
470
                case 't': // motor test
507
                case 't': // motor test
471
                        if (rxDataLen > 20) {
-
 
472
                                memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest));
508
                        memcpy(&outputTest[0], (uint8_t*) pRxData, /*sizeof(outputTest)*/ 12); // 12 is an mktool limitation.
473
                        } else {
-
 
474
                                memcpy(&motorTest[0], (uint8_t*) pRxData, 4);
509
                        outputTestActive = 255;
475
                        }
510
                        // Huh??
476
                        motorTestActive = 255;
-
 
477
                        externalControlActive = 255;
511
                        externalControlActive = 255;
478
                        break;
512
                        break;
Line 479... Line 513...
479
 
513
 
480
                case 'n':// Read motor mixer
514
                case 'n':// Read motor mixer
481
            tempchar[0] = EEMIXER_REVISION;
515
            tempchar[0] = EEMIXER_REVISION;
482
            tempchar[1] = sizeof(MotorMixer_t);
516
            tempchar[1] = sizeof(OutputMixer_t);
483
            while (!txd_complete)
517
            while (!txd_complete)
484
                ; // wait for previous frame to be sent
518
                ; // wait for previous frame to be sent
485
                        sendOutData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&motorMixer, sizeof(motorMixer));
519
                        sendData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&outputMixer, sizeof(OutputMixer_t));
Line 486... Line 520...
486
                        break;
520
                        break;
487
 
521
 
488
                case 'm':// "Set Mixer Table
522
                case 'm':// "Set Mixer Table
489
                        if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(MotorMixer_t))) {
523
                        if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(OutputMixer_t))) {
490
                                memcpy(&motorMixer, (uint8_t*)&pRxData[2], sizeof(motorMixer));
524
                                memcpy(&outputMixer, (uint8_t*)&pRxData[2], sizeof(OutputMixer_t));
491
                                motorMixer_writeToEEProm();
525
                                outputMixer_writeToEEProm();
492
                                while (!txd_complete)
526
                                while (!txd_complete)
493
                                        ; // wait for previous frame to be sent
527
                                        ; // wait for previous frame to be sent
494
                                tempchar[0] = 1;
528
                                tempchar[0] = 1;
495
                        } else {
529
                        } else {
496
                                tempchar[0] = 0;
530
                                tempchar[0] = 0;
497
                        }
531
                        }
Line 498... Line 532...
498
                        sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
532
                        sendData('M', FC_ADDRESS, 1, &tempchar, 1);
499
                        break;
533
                        break;
500
 
534
 
Line 505... Line 539...
505
        case 'i':// Read IMU configuration
539
        case 'i':// Read IMU configuration
506
            tempchar[0] = IMUCONFIG_REVISION;
540
            tempchar[0] = IMUCONFIG_REVISION;
507
            tempchar[1] = sizeof(IMUConfig);
541
            tempchar[1] = sizeof(IMUConfig);
508
            while (!txd_complete)
542
            while (!txd_complete)
509
                ; // wait for previous frame to be sent
543
                ; // wait for previous frame to be sent
510
            sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
544
            sendData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
511
            break;
545
            break;
Line 512... Line 546...
512
 
546
 
513
        case 'j':// Save IMU configuration
547
        case 'j':// Save IMU configuration
514
          if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
548
          if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
Line 520... Line 554...
520
              } else {
554
              } else {
521
                  tempchar[0] = 0; //indicate bad data
555
                  tempchar[0] = 0; //indicate bad data
522
              }
556
              }
523
              while (!txd_complete)
557
              while (!txd_complete)
524
                  ; // wait for previous frame to be sent
558
                  ; // wait for previous frame to be sent
525
              sendOutData('J', FC_ADDRESS, 1, &tempchar, 1);
559
              sendData('J', FC_ADDRESS, 1, &tempchar, 1);
526
          }
560
          }
527
          break;
561
          break;
Line 528... Line 562...
528
 
562
 
529
                case 'q':// request settings
563
                case 'q':// request settings
Line 542... Line 576...
542
                        tempchar[0] = pRxData[0];
576
                        tempchar[0] = pRxData[0];
543
                        tempchar[1] = EEPARAM_REVISION;
577
                        tempchar[1] = EEPARAM_REVISION;
544
                        tempchar[2] = sizeof(staticParams);
578
                        tempchar[2] = sizeof(staticParams);
545
                        while (!txd_complete)
579
                        while (!txd_complete)
546
                                ; // wait for previous frame to be sent
580
                                ; // wait for previous frame to be sent
547
                        sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
581
                        sendData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
548
                        break;
582
                        break;
Line 549... Line 583...
549
 
583
 
550
                case 's': // save settings
584
                case 's': // save settings
551
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
585
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
Line 557... Line 591...
557
                                        setActiveParamSet(pRxData[0]);
591
                                        setActiveParamSet(pRxData[0]);
558
                                        configuration_paramSetDidChange();
592
                                        configuration_paramSetDidChange();
559
                                        tempchar[0] = getActiveParamSet();
593
                                        tempchar[0] = getActiveParamSet();
560
                                        beepNumber(tempchar[0]);
594
                                        beepNumber(tempchar[0]);
561
                                } else {
595
                                } else {
562
                                        tempchar[0] = 0; //indicate bad data
596
                                        tempchar[0] = sizeof(staticParams); //indicate bad data
563
                                }
597
                                }
564
                                while (!txd_complete)
598
                                while (!txd_complete)
565
                                        ; // wait for previous frame to be sent
599
                                        ; // wait for previous frame to be sent
566
                                sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
600
                                sendData('S', FC_ADDRESS, 1, &tempchar, 1);
567
                        }
601
                        }
568
                        break;
602
                        break;
Line 569... Line 603...
569
 
603
 
570
                default:
604
                default:
Line 573... Line 607...
573
                } // case FC_ADDRESS:
607
                } // case FC_ADDRESS:
Line 574... Line 608...
574
 
608
 
575
        default: // any Slave Address
609
        default: // any Slave Address
576
                switch (rxd_buffer[2]) {
610
                switch (rxd_buffer[2]) {
577
                case 'a':// request for labels of the analog debug outputs
611
                case 'a':// request for labels of the analog debug outputs
578
                        requestedDebugLabel = pRxData[0];
612
                        requestedAnalogLabel = pRxData[0];
579
                        if (requestedDebugLabel > 31)
613
                        if (requestedAnalogLabel > 31)
580
                                requestedDebugLabel = 31;
614
                                requestedAnalogLabel = 31;
Line 581... Line 615...
581
                        break;
615
                        break;
582
 
616
 
583
                case 'b': // submit extern control
617
                case 'b': // submit extern control
584
                        memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
618
                        memcpy(&externalControl, (uint8_t*) pRxData, sizeof(ExternalControl_t));
585
                        confirmFrame = externalControl.frame;
619
                        confirmFrame = externalControl.frame;
Line 586... Line 620...
586
                        externalControlActive = 255;
620
                        externalControlActive = 255;
587
                        break;
621
                        break;
588
 
622
 
-
 
623
        case 'd': // request for the debug data
589
                case 'h':// request for display columns
624
            debugDataInterval = (uint16_t) pRxData[0] * 10;
-
 
625
            if (debugDataInterval > 0)
-
 
626
                request_debugData = TRUE;
590
                        remoteKeys |= pRxData[0];
627
            break;
591
                        if (remoteKeys)
628
 
Line -... Line 629...
-
 
629
        case 'e': // Requeset for the DCM matrix
-
 
630
            request_DCM_matrix = TRUE;
-
 
631
            break;
592
                                displayLine = 0;
632
 
593
                        request_display = TRUE;
633
        case 'f':
-
 
634
            requestedProfilerLabel = pRxData[0];
-
 
635
            if (requestedProfilerLabel > 15)
-
 
636
                requestedProfilerLabel = 15;
-
 
637
            break;
594
                        break;
638
 
595
 
639
        case 'u':
Line 596... Line 640...
596
                case 'l':// request for display columns
640
            profilerDataInterval = (uint16_t) pRxData[0] * 10;
597
                        menuItem = pRxData[0];
641
            if (profilerDataInterval > 0)
598
                        request_display1 = TRUE;
642
                request_profilerData = TRUE;
599
                        break;
643
            break;
600
 
644
 
Line 601... Line 645...
601
                case 'o':// request for OSD data (FC style)
645
                case 'o':// request for OSD data (FC style)
602
                  OSD_interval = (uint16_t) pRxData[0] * 10;
646
                  OSDDataInterval = (uint16_t) pRxData[0] * 10;
Line 614... Line 658...
614
 
658
 
615
                case 'g':// get external control data
659
                case 'g':// get external control data
616
                        request_externalControl = TRUE;
660
                        request_externalControl = TRUE;
Line 617... Line -...
617
                        break;
-
 
618
 
-
 
619
                case 'd': // request for the debug data
-
 
620
                        debugData_interval = (uint16_t) pRxData[0] * 10;
-
 
621
                        if (debugData_interval > 0)
-
 
622
                                request_debugData = TRUE;
-
 
623
                        break;
-
 
624
 
-
 
625
                case 'c': // request for the 3D data
-
 
626
                        data3D_interval = (uint16_t) pRxData[0] * 10;
-
 
627
                        if (data3D_interval > 0)
-
 
628
                                request_data3D = TRUE;
-
 
629
                        break;
661
                        break;
630
 
662
 
631
                default:
663
                default:
632
                        //unsupported command received
664
                        //unsupported command received
633
                        break;
665
                        break;
Line 641... Line 673...
641
}
673
}
Line 642... Line 674...
642
 
674
 
643
/************************************************************************/
675
/************************************************************************/
644
/* Routine f�r die Serielle Ausgabe                                     */
676
/* Routine f�r die Serielle Ausgabe                                     */
645
/************************************************************************/
677
/************************************************************************/
646
int16_t uart_putchar(int8_t c) {
678
int uart_putchar(char c, FILE* fims) {
647
        if (c == '\n')
679
        if (c == '\n')
648
                uart_putchar('\r');
680
                uart_putchar('\r', fims);
649
        // wait until previous character was send
681
        // wait until previous character was send
650
        loop_until_bit_is_set(UCSR0A, UDRE0);
682
        loop_until_bit_is_set(UCSR0A, UDRE0);
651
        // send character
683
        // send character
652
        UDR0 = c;
684
        UDR0 = c;
Line 657... Line 689...
657
void usart0_transmitTxData(void) {
689
void usart0_transmitTxData(void) {
658
        if (!txd_complete)
690
        if (!txd_complete)
659
                return;
691
                return;
Line 660... Line 692...
660
 
692
 
661
        if (request_verInfo && txd_complete) {
693
        if (request_verInfo && txd_complete) {
662
                sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
694
                sendData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
663
                request_verInfo = FALSE;
695
                request_verInfo = FALSE;
Line 664... Line 696...
664
        }
696
        }
665
 
-
 
666
        if (request_display && txd_complete) {
-
 
667
                LCD_printMenu();
-
 
668
                sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine),
-
 
669
                                &displayBuff[displayLine * 20], 20);
-
 
670
                displayLine++;
-
 
671
                if (displayLine >= 4)
697
 
672
                        displayLine = 0;
698
        if (request_display && txd_complete) {
Line 673... Line 699...
673
                request_display = FALSE;
699
                request_display = FALSE;
674
        }
-
 
675
 
-
 
676
        if (request_display1 && txd_complete) {
-
 
677
                LCD_printMenu();
700
        }
678
                sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem,
701
 
Line 679... Line 702...
679
                                sizeof(maxMenuItem), displayBuff, sizeof(displayBuff));
702
        if (request_display1 && txd_complete) {
680
                request_display1 = FALSE;
703
                request_display1 = FALSE;
-
 
704
        }
681
        }
705
 
682
 
706
        if (requestedAnalogLabel != 0xFF && txd_complete) {
683
        if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
707
                char label[17]; // local sram buffer
-
 
708
                memset(label, ' ', sizeof(label));
-
 
709
                strcpy_P(label, (PGM_P)pgm_read_word(&(ANALOG_LABELS[requestedAnalogLabel]))); // read label from flash to sram buffer
-
 
710
                sendData('A', FC_ADDRESS, 2, (uint8_t *) &requestedAnalogLabel, sizeof(requestedAnalogLabel), label, 16);
-
 
711
                requestedAnalogLabel = 0xFF;
-
 
712
        }
-
 
713
 
-
 
714
    if (requestedProfilerLabel != 0xFF && txd_complete) {
684
                uint8_t label[16]; // local sram buffer
715
        char label[17]; // local sram buffer
685
                memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
716
        memset(label, ' ', sizeof(label));
Line 686... Line 717...
686
                sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
717
        strcpy_P(label, (PGM_P)pgm_read_word(&(PROFILER_LABELS[requestedProfilerLabel]))); // read label from flash to sram buffer
687
                                sizeof(requestedDebugLabel), label, 16);
718
        sendData('F', FC_ADDRESS, 2, (uint8_t *) &requestedProfilerLabel, sizeof(requestedProfilerLabel), label, 16);
688
                requestedDebugLabel = 0xFF;
719
        requestedProfilerLabel = 0xFF;
689
        }
720
    }
Line 690... Line 721...
690
 
721
 
691
        if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
-
 
692
                sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
722
        if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
693
                confirmFrame = 0;
723
                sendData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
694
        }
724
                confirmFrame = 0;
695
 
725
        }
Line 696... Line 726...
696
        if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData)
726
 
697
                        && txd_complete) {
727
        if (((debugDataInterval && checkDelay(debugDataTimer)) || request_debugData) && txd_complete) {
698
                sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
728
                sendData('D', FC_ADDRESS, 1, (uint8_t *)&debugOut, sizeof(debugOut));
699
                debugData_timer = setDelay(debugData_interval);
729
                debugDataTimer = setDelay(debugDataInterval);
-
 
730
                request_debugData = FALSE;
-
 
731
        }
700
                request_debugData = FALSE;
732
 
-
 
733
    if (((profilerDataInterval && checkDelay(profilerDataTimer)) || request_profilerData) && txd_complete) {
-
 
734
        sendData('U', FC_ADDRESS, 2, (uint8_t *)&totalProfilerHits, sizeof(totalProfilerHits), (uint8_t *)&activitiesTimerHits, sizeof(activitiesTimerHits));
701
        }
735
        profilerDataTimer = setDelay(profilerDataInterval);
-
 
736
        request_profilerData = FALSE;
702
 
737
    }
703
        if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
738
 
Line 704... Line 739...
704
                sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
739
        if (request_DCM_matrix && txd_complete) {
705
                data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
740
                /*
706
                data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
741
                sendData('E', FC_ADDRESS, 1,
707
                data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
742
                                (uint8_t *) &dcmGyro, sizeof(dcmGyro));
708
                data3D_timer = setDelay(data3D_interval);
743
                */
Line 709... Line 744...
709
                request_data3D = FALSE;
744
                request_DCM_matrix = FALSE;
Line 720... Line 755...
720
                toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
755
                toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
721
                toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
756
                toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
722
                toMk3Mag.userParam[0] = dynamicParams.userParams[0];
757
                toMk3Mag.userParam[0] = dynamicParams.userParams[0];
723
                toMk3Mag.userParam[1] = dynamicParams.userParams[1];
758
                toMk3Mag.userParam[1] = dynamicParams.userParams[1];
724
                toMk3Mag.calState = compassCalState;
759
                toMk3Mag.calState = compassCalState;
725
                sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
760
                sendData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
726
                // the last state is 5 and should be send only once to avoid multiple flash writing
761
                // the last state is 5 and should be send only once to avoid multiple flash writing
727
                if(compassCalState > 4) compassCalState = 0;
762
                if(compassCalState > 4) compassCalState = 0;
728
                toMk3MagTimer = setDelay(99);
763
                toMk3MagTimer = setDelay(99);
729
        }
764
        }
730
#endif
765
#endif
Line 731... Line 766...
731
 
766
 
732
        if (request_motorTest && txd_complete) {
767
        if (request_outputTest && txd_complete) {
733
                sendOutData('T', FC_ADDRESS, 0);
768
                sendData('T', FC_ADDRESS, 0);
734
                request_motorTest = FALSE;
769
                request_outputTest = FALSE;
Line 735... Line 770...
735
        }
770
        }
736
 
771
 
737
        if (request_PPMChannels && txd_complete) {
772
        if (request_PPMChannels && txd_complete) {
738
                uint8_t length = MAX_CHANNELS;
773
                uint8_t length = MAX_CONTROLCHANNELS;
739
                sendOutData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
774
                sendData('P', FC_ADDRESS, 2, &length, 1, (uint8_t*)&PPM_in, sizeof(PPM_in));
Line 740... Line 775...
740
                request_PPMChannels = FALSE;
775
                request_PPMChannels = FALSE;
741
        }
776
        }
742
 
777
 
743
        if (request_variables && txd_complete) {
778
        if (request_variables && txd_complete) {
Line 744... Line 779...
744
                sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
779
                sendData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
745
                request_variables = FALSE;
780
                request_variables = FALSE;
746
        }
781
        }
747
 
782
 
748
#ifdef USE_DIRECT_GPS
783
#ifdef USE_DIRECT_GPS
749
        if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
784
        if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
750
          int32_t height = analog_getHeight();
785
          int32_t height = analog_getHeight();
751
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
786
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
752
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
787
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
753
          data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
788
          data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
754
          sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
789
          sendData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
755
          OSD_timer = setDelay(OSD_interval);
790
          OSD_timer = setDelay(OSD_interval);