Rev 2045 | Rev 2051 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2045 | Rev 2048 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | #include "uart0.h" |
11 | #include "uart0.h" |
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" |
- | 16 | #include "commands.h" |
|
Line 16... | Line 17... | ||
16 | 17 | ||
17 | #ifdef USE_DIRECT_GPS |
18 | #ifdef USE_DIRECT_GPS |
18 | #include "mk3mag.h" |
19 | #include "mk3mag.h" |
Line 94... | Line 95... | ||
94 | "TargetHeight ", |
95 | "TargetHeight ", |
95 | "heightError ", |
96 | "heightError ", |
96 | "HeightPdThrottle", |
97 | "HeightPdThrottle", |
97 | "HeightIdThrottle", //25 |
98 | "HeightIdThrottle", //25 |
98 | "HeightDdThrottle", |
99 | "HeightDdThrottle", |
99 | "NaviStickPitch ", |
100 | "HeadingInDegrees", |
100 | "NaviStickRoll ", |
101 | "TargetHeadingDeg", |
101 | "StickPitch ", |
102 | "Diff ", |
102 | "StickRoll ", //30 |
103 | " ", //30 |
103 | "navi mode " |
104 | "navi mode " |
104 | }; |
105 | }; |
Line 105... | Line 106... | ||
105 | 106 | ||
106 | /****************************************************************/ |
107 | /****************************************************************/ |
Line 652... | Line 653... | ||
652 | } |
653 | } |
Line 653... | Line 654... | ||
653 | 654 | ||
654 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
655 | if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) |
655 | && txd_complete) { |
656 | && txd_complete) { |
656 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
657 | sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
657 | data3D.anglePitch = (int16_t) (angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
658 | data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
658 | data3D.angleRoll = (int16_t) (angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
659 | data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1° |
659 | data3D.heading = (int16_t) (yawGyroHeading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
660 | data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1° |
660 | data3D_timer = setDelay(data3D_interval); |
661 | data3D_timer = setDelay(data3D_interval); |
661 | request_data3D = FALSE; |
662 | request_data3D = FALSE; |
Line 662... | Line 663... | ||
662 | } |
663 | } |
Line 667... | Line 668... | ||
667 | request_externalControl = FALSE; |
668 | request_externalControl = FALSE; |
668 | } |
669 | } |
Line 669... | Line 670... | ||
669 | 670 | ||
670 | #ifdef USE_DIRECT_GPS |
671 | #ifdef USE_DIRECT_GPS |
671 | if((checkDelay(toMk3MagTimer)) && txd_complete) { |
672 | if((checkDelay(toMk3MagTimer)) && txd_complete) { |
672 | toMk3Mag.attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
673 | toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
673 | toMk3Mag.attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
674 | toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg |
674 | toMk3Mag.userParam[0] = dynamicParams.userParams[0]; |
675 | toMk3Mag.userParam[0] = dynamicParams.userParams[0]; |
675 | toMk3Mag.userParam[1] = dynamicParams.userParams[1]; |
676 | toMk3Mag.userParam[1] = dynamicParams.userParams[1]; |
676 | toMk3Mag.calState = compassCalState; |
677 | toMk3Mag.calState = compassCalState; |
677 | sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
678 | sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |