Subversion Repositories FlightCtrl

Rev

Rev 2035 | Rev 2041 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2035 Rev 2039
Line 12... Line 12...
12 
#include "rc.h"
 12 
#include "rc.h"
13 
#include "externalControl.h"
 13 
#include "externalControl.h"
14 
#include "output.h"
 14 
#include "output.h"
15 
#include "attitude.h"
 15 
#include "attitude.h"
Line 16... Line 16...
16 
 
 16 
 
17 
#ifdef USE_MK3MAG
 17 
#ifdef USE_DIRECT_GPS
18 
#include "mk3mag.h"
 18 
#include "mk3mag.h"
Line 19... Line 19...
19 
#endif
 19 
#endif
20 
 
 20 
 
Line 49... Line 49...
49 
uint8_t motorTestActive = 0;
 49 
uint8_t motorTestActive = 0;
50 
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 50 
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
51 
uint8_t confirmFrame;
 51 
uint8_t confirmFrame;
Line 52... Line 52...
52 
 
 52 
 
53 
typedef struct {
 53 
typedef struct {
54 
        int16_t Heading;
 54 
        int16_t heading;
Line 55... Line 55...
55 
}__attribute__((packed)) Heading_t;
 55 
}__attribute__((packed)) Heading_t;
56 
 
 56 
 
Line 57... Line 57...
57 
DebugOut_t debugOut;
 57 
DebugOut_t debugOut;
58 
Data3D_t data3D;
 58 
Data3D_t data3D;
59 
 
 59 
 
60 
uint16_t debugData_timer;
 60 
uint16_t debugData_timer;
Line 61... Line 61...
61 
uint16_t data3D_timer;
 61 
uint16_t data3D_timer;
62 
uint16_t debugData_interval = 0; // in 1ms
 62 
uint16_t debugData_interval = 0; // in 1ms
63 
uint16_t data3D_interval = 0; // in 1ms
 63 
uint16_t data3D_interval = 0; // in 1ms
Line 64... Line 64...
64 
 
 64 
 
65 
#ifdef USE_MK3MAG
 65 
#ifdef USE_DIRECT_GPS
66 
int16_t compass_timer;
 66 
int16_t toMk3MagTimer;
Line 168... Line 168...
168 
        rxDataLen = 0;
 168 
        rxDataLen = 0;
Line 169... Line 169...
169 
 
 169 
 
170 
        // no bytes to send
 170 
        // no bytes to send
Line 171... Line 171...
171 
        txd_complete = TRUE;
 171 
        txd_complete = TRUE;
172 
 
 172 
 
173 
#ifdef USE_MK3MAG
 173 
#ifdef USE_DIRECT_GPS
Line 174... Line 174...
174 
        compass_timer = setDelay(220);
 174 
        toMk3MagTimer = setDelay(220);
175 
#endif
 175 
#endif
176 
 
 176 
 
Line 444... Line 444...
444 
 
 444 
 
Line 445... Line 445...
445 
        switch (rxd_buffer[1] - 'a') {
 445 
        switch (rxd_buffer[1] - 'a') {
446 
 
 446 
 
447 
        case FC_ADDRESS:
 447 
        case FC_ADDRESS:
448 
                switch (rxd_buffer[2]) {
 448 
                switch (rxd_buffer[2]) {
449 
#ifdef USE_MK3MAG
 449 
#ifdef USE_DIRECT_GPS
450 
                case 'K':// compass value
 450 
                case 'K':// compass value
451 
                compassHeading = ((Heading_t *)pRxData)->Heading;
 451 
                compassHeading = ((Heading_t *)pRxData)->heading;
452 
                // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
 452 
                // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
453 
                break;
 453 
                break;
454 
#endif
 454 
#endif
Line 665... Line 665...
665 
                sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
 665 
                sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
666 
                                sizeof(externalControl));
 666 
                                sizeof(externalControl));
667 
                request_externalControl = FALSE;
 667 
                request_externalControl = FALSE;
668 
        }
 668 
        }
Line 669... Line 669...
669 
 
 669 
 
670 
#ifdef USE_MK3MAG
 670 
#ifdef USE_DIRECT_GPS
671 
        if((checkDelay(Compass_Timer)) && txd_complete) {
 671 
        if((checkDelay(toMk3MagTimer)) && txd_complete) {
672 
                toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
 672 
                toMk3Mag.attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
673 
                toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
 673 
                toMk3Mag.attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
674 
                toMk3Mag.UserParam[0] = dynamicParams.userParams[0];
 674 
                toMk3Mag.userParam[0] = dynamicParams.userParams[0];
675 
                toMk3Mag.UserParam[1] = dynamicParams.userParams[1];
 675 
                toMk3Mag.userParam[1] = dynamicParams.userParams[1];
676 
                toMk3Mag.CalState = compassCalState;
 676 
                toMk3Mag.calState = compassCalState;
677 
                sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
 677 
                sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
678 
                // the last state is 5 and should be send only once to avoid multiple flash writing
 678 
                // the last state is 5 and should be send only once to avoid multiple flash writing
679 
                if(compassCalState > 4) compassCalState = 0;
 679 
                if(compassCalState > 4) compassCalState = 0;
680 
                compass_timer = setDelay(99);
 680 
                toMk3MagTimer = setDelay(99);
681 
        }
 681 
        }
Line 682... Line 682...
682 
#endif
 682 
#endif
683 
 
 683