Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1227 | killagreg | 1 | /*####################################################################################### |
2 | Flight Control |
||
3 | #######################################################################################*/ |
||
4 | |||
5 | #ifndef _FC_H |
||
6 | #define _FC_H |
||
7 | |||
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), |
||
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 |
||
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 |
||
26 | #define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR) |
||
27 | |||
28 | |||
29 | extern uint8_t RequiredMotors; |
||
30 | |||
31 | typedef struct |
||
32 | { |
||
33 | uint8_t HeightD; |
||
34 | uint8_t MaxHeight; |
||
35 | uint8_t HeightP; |
||
36 | uint8_t Height_ACC_Effect; |
||
37 | uint8_t CompassYawEffect; |
||
38 | uint8_t GyroD; |
||
39 | uint8_t GyroP; |
||
40 | uint8_t GyroI; |
||
41 | uint8_t StickYawP; |
||
42 | uint8_t IFactor; |
||
43 | uint8_t UserParam1; |
||
44 | uint8_t UserParam2; |
||
45 | uint8_t UserParam3; |
||
46 | uint8_t UserParam4; |
||
47 | uint8_t UserParam5; |
||
48 | uint8_t UserParam6; |
||
49 | uint8_t UserParam7; |
||
50 | uint8_t UserParam8; |
||
51 | uint8_t ServoNickControl; |
||
52 | uint8_t LoopGasLimit; |
||
53 | uint8_t AxisCoupling1; |
||
54 | uint8_t AxisCoupling2; |
||
55 | uint8_t AxisCouplingYawCorrection; |
||
56 | uint8_t DynamicStability; |
||
57 | uint8_t ExternalControl; |
||
58 | uint8_t J16Timing; |
||
59 | uint8_t J17Timing; |
||
60 | #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
||
61 | uint8_t NaviGpsModeControl; |
||
62 | uint8_t NaviGpsGain; |
||
63 | uint8_t NaviGpsP; |
||
64 | uint8_t NaviGpsI; |
||
65 | uint8_t NaviGpsD; |
||
66 | uint8_t NaviGpsACC; |
||
67 | uint8_t NaviOperatingRadius; |
||
68 | uint8_t NaviWindCorrection; |
||
69 | uint8_t NaviSpeedCompensation; |
||
70 | #endif |
||
71 | int8_t KalmanK; |
||
72 | int8_t KalmanMaxDrift; |
||
73 | int8_t KalmanMaxFusion; |
||
74 | } fc_param_t; |
||
75 | |||
76 | extern fc_param_t FCParam; |
||
77 | |||
78 | |||
79 | // rotation rates |
||
80 | extern int16_t GyroNick, GyroRoll, GyroYaw; |
||
81 | |||
82 | // attitude calcualted by temporal integral of gyro rates |
||
83 | extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw; |
||
84 | |||
85 | |||
86 | // bias values |
||
87 | extern int16_t BiasHiResGyroNick, BiasHiResGyroRoll, AdBiasGyroYaw; |
||
88 | extern int16_t AdBiasAccNick, AdBiasAccRoll; |
||
89 | extern volatile float AdBiasAccTop; |
||
90 | |||
91 | extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
||
92 | |||
93 | // compass navigation |
||
94 | extern int16_t CompassHeading; |
||
95 | extern int16_t CompassCourse; |
||
96 | extern int16_t CompassOffCourse; |
||
97 | extern uint8_t CompassCalState; |
||
98 | extern int32_t YawGyroHeading; |
||
99 | extern int16_t YawGyroHeadingInDeg; |
||
100 | |||
101 | // hight control |
||
102 | extern int ReadingHeight; |
||
103 | extern int SetPointHeight; |
||
104 | |||
105 | // accelerations |
||
106 | extern int16_t AccNick, AccRoll, AccTop; |
||
107 | |||
108 | // acceleration send to navi board |
||
109 | extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
||
110 | |||
111 | |||
112 | // looping params |
||
113 | extern long TurnOver180Nick, TurnOver180Roll; |
||
114 | |||
115 | // external control |
||
116 | extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
||
117 | |||
118 | #define ACC_CALIB 1 |
||
119 | #define NO_ACC_CALIB 0 |
||
120 | |||
121 | void MotorControl(void); |
||
122 | void SendMotorData(void); |
||
123 | void SetNeutral(uint8_t AccAdjustment); |
||
124 | void Beep(uint8_t numbeeps); |
||
125 | |||
126 | |||
127 | extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
||
128 | |||
129 | // current stick values |
||
130 | extern int16_t StickNick; |
||
131 | extern int16_t StickRoll; |
||
132 | extern int16_t StickYaw; |
||
133 | // current GPS-stick values |
||
134 | extern int16_t GPSStickNick; |
||
135 | extern int16_t GPSStickRoll; |
||
136 | |||
137 | // current stick elongations |
||
138 | extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
||
139 | |||
140 | |||
141 | extern uint16_t ModelIsFlying; |
||
142 | |||
143 | |||
144 | // MKFlags |
||
145 | #define MKFLAG_MOTOR_RUN 0x01 |
||
146 | #define MKFLAG_FLY 0x02 |
||
147 | #define MKFLAG_CALIBRATE 0x04 |
||
148 | #define MKFLAG_START 0x08 |
||
149 | #define MKFLAG_EMERGENCY_LANDING 0x10 |
||
150 | #define MKFLAG_RESERVE1 0x20 |
||
151 | #define MKFLAG_RESERVE2 0x40 |
||
152 | #define MKFLAG_RESERVE3 0x80 |
||
153 | |||
154 | extern volatile uint8_t MKFlags; |
||
155 | |||
156 | #endif //_FC_H |
||
157 |