Subversion Repositories FlightCtrl

Rev

Rev 1179 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1179 Rev 1180
Line 5... Line 5...
5 
#ifndef _FC_H
 5 
#ifndef _FC_H
6 
#define _FC_H
 6 
#define _FC_H
Line 7... Line 7...
7 
 
 7 
 
Line -... Line 8...
- 
 
 8 
#include <inttypes.h>
- 
 
 9 
 
- 
 
 10 
// scaling from AdAccNick, AdAccRoll -> AccNick, AccRoll
- 
 
 11 
// i.e. AccNick = ACC_AMPLIFY * AdAccNick
- 
 
 12 
#define ACC_AMPLIFY             6
- 
 
 13 
 
- 
 
 14 
// scaling from AccNick, AccRoll -> Attitude in deg (approx sin(x) = x),
8 
#include <inttypes.h>
 15 
// i.e. Nick Angle in deg = AccNick / ACC_DEG_FACTOR
- 
 
 16 
 
- 
 
 17 
// the value is derived from the datasheet of the ACC sensor where 5g are scaled to vref
- 
 
 18 
// therefore 1g is 1024/5 = 205 counts. the adc isr combines 2 acc samples to AdValueAcc
- 
 
 19 
// and 1g yields to AdValueAcc = 2* 205 * 410 wich is again scaled by ACC_DEG_FACTOR
9 
 
 20 
// that results in 1g --> Acc = 205 * 12 = 2460. the linear approx of the arcsin and the scaling
- 
 
 21 
// of Acc gives the factor below. sin(20deg) * 2460 = 841 --> 841 / 20 = 42
- 
 
 22 
#define ACC_DEG_FACTOR     42
- 
 
 23 
 
- 
 
 24 
// scaling from IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw -> Attitude in deg
- 
 
 25 
// i.e. Nick Angle in deg = IntegralGyroNick / GYRO_DEG_FACTOR
Line 10... Line 26...
10 
#define YAW_GYRO_DEG_FACTOR 1291L // Factor between Yaw Gyro Integral and HeadingAngle in deg
 26 
#define GYRO_DEG_FACTOR    ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR)
11 
#define STICK_GAIN 4
 27 
 
12 
 
 28 
 
13 
typedef struct
 29 
typedef struct
14 
{
 30 
{
15 
        uint8_t Height_D;
 31 
        uint8_t HeightD;
16 
        uint8_t MaxHeight;
 32 
        uint8_t MaxHeight;
- 
 
 33 
        uint8_t HeightP;
17 
        uint8_t Height_P;
 34 
        uint8_t Height_ACC_Effect;
18 
        uint8_t Height_ACC_Effect;
 35 
        uint8_t CompassYawEffect;
19 
        uint8_t CompassYawEffect;
 36 
        uint8_t GyroD;
20 
        uint8_t Gyro_P;
 37 
        uint8_t GyroP;
21 
        uint8_t Gyro_I;
 38 
        uint8_t GyroI;
22 
        uint8_t Yaw_P;
 39 
        uint8_t StickYawP;
23 
        uint8_t I_Factor;
 40 
        uint8_t IFactor;
24 
        uint8_t UserParam1;
 41 
        uint8_t UserParam1;
25 
        uint8_t UserParam2;
 42 
        uint8_t UserParam2;
26 
        uint8_t UserParam3;
 43 
        uint8_t UserParam3;
27 
        uint8_t UserParam4;
 44 
        uint8_t UserParam4;
28 
        uint8_t UserParam5;
 45 
        uint8_t UserParam5;
29 
        uint8_t UserParam6;
 46 
        uint8_t UserParam6;
30 
        uint8_t UserParam7;
 47 
        uint8_t UserParam7;
31 
        uint8_t UserParam8;
 48 
        uint8_t UserParam8;
32 
        uint8_t ServoNickControl;
 49 
        uint8_t ServoNickControl;
- 
 
 50 
        uint8_t LoopGasLimit;
33 
        uint8_t LoopGasLimit;
 51 
        uint8_t AxisCoupling1;
34 
        uint8_t Yaw_PosFeedback;
 52 
        uint8_t AxisCoupling2;
35 
        uint8_t Yaw_NegFeedback;
 53 
        uint8_t AxisCouplingYawCorrection;
36 
        uint8_t DynamicStability;
 54 
        uint8_t DynamicStability;
37 
        uint8_t ExternalControl;
 55 
        uint8_t ExternalControl;
Line 46... Line 64...
46 
        uint8_t NaviGpsACC;
 64 
        uint8_t NaviGpsACC;
47 
        uint8_t NaviOperatingRadius;
 65 
        uint8_t NaviOperatingRadius;
48 
        uint8_t NaviWindCorrection;
 66 
        uint8_t NaviWindCorrection;
49 
        uint8_t NaviSpeedCompensation;
 67 
        uint8_t NaviSpeedCompensation;
50 
        #endif
 68 
        #endif
51 
        int8_t Kalman_K;
 69 
        int8_t KalmanK;
52 
        int8_t Kalman_MaxDrift;
 70 
        int8_t KalmanMaxDrift;
53 
        int8_t Kalman_MaxFusion;
 71 
        int8_t KalmanMaxFusion;
54 
} fc_param_t;
 72 
} fc_param_t;
Line 55... Line 73...
55 
 
 73 
 
Line 56... Line -...
56 
extern fc_param_t FCParam;
 - 
 
57 
 
 - 
 
58 
// attitude
 - 
 
Line 59... Line 74...
59 
extern  int32_t IntegralNick, IntegralRoll, IntegralYaw;
 74 
extern fc_param_t FCParam;
60 
extern  int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
 75 
 
- 
 
 76 
 
61 
 
 77 
// rotation rates
62 
// offsets
 78 
extern  int16_t GyroNick, GyroRoll, GyroYaw;
- 
 
 79 
 
Line -... Line 80...
- 
 
 80 
// attitude calcualted by temporal integral of gyro rates
- 
 
 81 
extern  int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw;
- 
 
 82 
 
- 
 
 83 
 
Line 63... Line 84...
63 
extern  int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw;
 84 
// bias values
Line 64... Line 85...
64 
extern volatile int16_t NeutralAccX, NeutralAccY;
 85 
extern int16_t BiasHiResGyroNick, BiasHiResGyroRoll, AdBiasGyroYaw;
65 
extern volatile float NeutralAccZ;
 86 
extern int16_t AdBiasAccNick, AdBiasAccRoll;
66 
 
 87 
extern volatile float AdBiasAccTop;
67 
 
 88 
 
68 
extern volatile int32_t Reading_Integral_Top; // calculated in analog.c
 89 
extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
69 
 
 90 
 
70 
// compass navigation
 91 
// compass navigation
Line 71... Line 92...
71 
extern volatile int16_t CompassHeading;
 92 
extern int16_t CompassHeading;
72 
extern volatile int16_t CompassCourse;
 93 
extern int16_t CompassCourse;
73 
extern volatile int16_t CompassOffCourse;
 94 
extern int16_t CompassOffCourse;
Line 74... Line 95...
74 
extern volatile uint8_t CompassCalState;
 95 
extern uint8_t CompassCalState;
75 
extern int32_t YawGyroHeading;
 96 
extern int32_t YawGyroHeading;
Line 76... Line 97...
76 
extern int16_t YawGyroHeadingInDeg;
 97 
extern int16_t YawGyroHeadingInDeg;
77 
 
 98 
 
Line 90... Line 111...
90 
extern long TurnOver180Nick, TurnOver180Roll;
 111 
extern long TurnOver180Nick, TurnOver180Roll;
Line 91... Line 112...
91 
 
 112 
 
92 
// external control
 113 
// external control
Line -... Line 114...
- 
 
 114 
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw;
- 
 
 115 
 
Line 93... Line 116...
93 
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw;
 116 
#define ACC_CALIB 1
94 
 
 117 
#define NO_ACC_CALIB 0
95 
 
 - 
 
96 
void MotorControl(void);
 - 
 
97 
void SendMotorData(void);
 118 
 
98 
void CalibMean(void);
 119 
void MotorControl(void);
Line 99... Line 120...
99 
void Mean(void);
 120 
void SendMotorData(void);
Line 100... Line 121...
100 
void SetNeutral(void);
 121 
void SetNeutral(uint8_t AccAdjustment);
101 
void Beep(uint8_t numbeeps);
 122 
void Beep(uint8_t numbeeps);
Line 102... Line 123...
102 
 
 123 
 
103 
 
 124 
 
104 
extern int16_t  Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8;
 125 
extern int16_t  Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8;
105 
 
 126 
 
- 
 
 127 
// setpoints for motors
106 
// setpoints for motors
 128 
extern volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;
107 
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
 129 
 
Line 108... Line 130...
108 
 
 130 
// current stick values
109 
// current stick values
 131 
extern int16_t StickNick;
Line 110... Line 132...
110 
extern int16_t StickNick;
 132 
extern int16_t StickRoll;
Line 111... Line 133...
111 
extern int16_t StickRoll;
 133 
extern int16_t StickYaw;
112 
extern int16_t StickYaw;
 134 
// current GPS-stick values
113 
extern int16_t GPS_Nick;
 135 
extern int16_t GPSStickNick;
Line 128... Line 150...
128 
#define MKFLAG_EMERGENCY_LANDING        0x10
 150 
#define MKFLAG_EMERGENCY_LANDING        0x10
129 
#define MKFLAG_RESERVE1                         0x20
 151 
#define MKFLAG_RESERVE1                         0x20
130 
#define MKFLAG_RESERVE2                         0x40
 152 
#define MKFLAG_RESERVE2                         0x40
131 
#define MKFLAG_RESERVE3                         0x80
 153 
#define MKFLAG_RESERVE3                         0x80
Line 132... Line 154...
132 
 
 154 
 
Line 133... Line 155...
133 
volatile extern uint8_t MKFlags;
 155 
extern uint8_t MKFlags;