Subversion Repositories FlightCtrl

Rev

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));