Rev 2099 | Rev 2103 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2099 | Rev 2102 | ||
---|---|---|---|
Line 5... | Line 5... | ||
5 | #include "output.h" |
5 | #include "output.h" |
Line 6... | Line 6... | ||
6 | 6 | ||
7 | // Necessary for external control and motor test |
7 | // Necessary for external control and motor test |
Line 8... | Line -... | ||
8 | #include "uart0.h" |
- | |
9 | - | ||
10 | // for scope debugging |
- | |
11 | // #include "rc.h" |
8 | #include "uart0.h" |
12 | 9 | ||
13 | #include "timer2.h" |
10 | #include "timer2.h" |
Line 14... | Line 11... | ||
14 | #include "attitude.h" |
11 | #include "attitude.h" |
Line 30... | Line 27... | ||
30 | uint8_t dFactor[3]; |
27 | uint8_t dFactor[3]; |
31 | uint8_t iFactor[3]; |
28 | uint8_t iFactor[3]; |
32 | uint8_t reverse[3]; |
29 | uint8_t reverse[3]; |
33 | int32_t IPart[3] = { 0, 0, 0 }; |
30 | int32_t IPart[3] = { 0, 0, 0 }; |
Line 34... | Line 31... | ||
34 | 31 | ||
Line 35... | Line 32... | ||
35 | int16_t servos[MAX_SERVOS]; |
32 | int16_t controlServos[MAX_CONTROL_SERVOS]; |
36 | 33 | ||
37 | /************************************************************************/ |
34 | /************************************************************************/ |
38 | /* Neutral Readings */ |
35 | /* Neutral Readings */ |
Line 39... | Line 36... | ||
39 | /************************************************************************/ |
36 | /************************************************************************/ |
40 | #define CONTROL_CONFIG_SCALE 10 |
37 | #define CONTROL_CONFIG_SCALE 10 |
41 | 38 | ||
42 | void flight_setGround(void) { |
39 | void flight_setGround(void) { |
43 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
40 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
44 | target[PITCH] = attitude[PITCH]; |
41 | target[PITCH] = attitude[PITCH]; |
Line 45... | Line 42... | ||
45 | target[ROLL] = attitude[ROLL]; |
42 | target[ROLL] = attitude[ROLL]; |
46 | target[YAW] = attitude[YAW]; |
43 | target[YAW] = attitude[YAW]; |
47 | } |
44 | } |
- | 45 | ||
48 | 46 | // this should be followed by a call to switchToFlightMode!! |
|
49 | void switchToFlightMode(FlightMode_t flightMode) { |
47 | void updateFlightParametersToFlightMode(uint8_t flightMode) { |
50 | if (flightMode == FLIGHT_MODE_MANUAL) { |
48 | debugOut.analog[15] = flightMode; |
51 | pFactor[PITCH] = 0; |
49 | |
52 | pFactor[ROLL] = 0; |
50 | reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse |
53 | pFactor[YAW] = 0; |
51 | & CONTROL_SERVO_REVERSE_ELEVATOR; |
54 | } else if (flightMode == FLIGHT_MODE_RATE) { |
52 | reverse[CONTROL_AILERONS] = staticParams.controlServosReverse |
- | 53 | & CONTROL_SERVO_REVERSE_AILERONS; |
|
55 | pFactor[PITCH] = 0; //staticParams...; |
54 | reverse[CONTROL_RUDDER] = staticParams.controlServosReverse |
56 | pFactor[ROLL] = 0; //staticParams...; |
- | |
57 | pFactor[YAW] = 0; //staticParams...; |
55 | & CONTROL_SERVO_REVERSE_RUDDER; |
58 | } |
56 | |
59 | if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
57 | for (uint8_t i = 0; i < 3; i++) { |
60 | iFactor[PITCH] = 0; |
- | |
61 | iFactor[ROLL] = 0; |
58 | if (flightMode == FLIGHT_MODE_MANUAL) { |
62 | iFactor[YAW] = 0; |
59 | pFactor[i] = 0; |
63 | } else if (flightMode == FLIGHT_MODE_ANGLES) { |
- | |
64 | iFactor[PITCH] = 0; //staticParams...; |
- | |
65 | iFactor[ROLL] = 0; //staticParams...; |
- | |
66 | iFactor[YAW] = 0; //staticParams...; |
- | |
67 | // When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
60 | dFactor[i] = 0; |
68 | target[PITCH] = attitude[PITCH]; |
61 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
69 | target[ROLL] = attitude[ROLL]; |
62 | pFactor[i] = staticParams.gyroPID[i].P; |
70 | target[YAW] = attitude[YAW]; |
63 | dFactor[i] = staticParams.gyroPID[i].D; |
71 | } |
64 | } |
- | 65 | ||
- | 66 | if (flightMode == FLIGHT_MODE_ANGLES) { |
|
72 | 67 | iFactor[i] = staticParams.gyroPID[i].I; |
|
73 | dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
- | |
74 | dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
68 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
Line 75... | Line 69... | ||
75 | dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
69 | iFactor[i] = 0; |
76 | 70 | } |
|
77 | // TODO: Set reverse also. |
71 | } |
78 | } |
72 | } |
79 | 73 | ||
80 | /************************************************************************/ |
74 | /************************************************************************/ |
Line 81... | Line 75... | ||
81 | /* Main Flight Control */ |
75 | /* Main Flight Control */ |
82 | /************************************************************************/ |
76 | /************************************************************************/ |
Line 83... | Line 77... | ||
83 | void flight_control(void) { |
77 | void flight_control(void) { |
Line 192... | Line 196... | ||
192 | debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
196 | debugOut.analog[13] = term[ROLL]; |