Rev 2099 | Rev 2110 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2099 | Rev 2103 | ||
---|---|---|---|
1 | #include <stdlib.h> |
1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
2 | #include <avr/io.h> |
3 | #include <stdlib.h> |
3 | #include <stdlib.h> |
4 | 4 | ||
5 | #include "attitude.h" |
5 | #include "attitude.h" |
6 | #include "dongfangMath.h" |
6 | #include "dongfangMath.h" |
7 | #include "commands.h" |
7 | #include "commands.h" |
8 | 8 | ||
9 | // For scope debugging only! |
9 | // For scope debugging only! |
10 | #include "rc.h" |
10 | #include "rc.h" |
11 | 11 | ||
12 | // where our main data flow comes from. |
12 | // where our main data flow comes from. |
13 | #include "analog.h" |
13 | #include "analog.h" |
14 | 14 | ||
15 | #include "configuration.h" |
15 | #include "configuration.h" |
16 | #include "output.h" |
16 | #include "output.h" |
17 | 17 | ||
18 | // Some calculations are performed depending on some stick related things. |
18 | // Some calculations are performed depending on some stick related things. |
19 | #include "controlMixer.h" |
19 | #include "controlMixer.h" |
20 | 20 | ||
21 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
21 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
22 | 22 | ||
23 | /* |
23 | /* |
24 | * Gyro readings, as read from the analog module. It would have been nice to flow |
24 | * Gyro readings, as read from the analog module. It would have been nice to flow |
25 | * them around between the different calculations as a struct or array (doing |
25 | * them around between the different calculations as a struct or array (doing |
26 | * things functionally without side effects) but this is shorter and probably |
26 | * things functionally without side effects) but this is shorter and probably |
27 | * faster too. |
27 | * faster too. |
28 | * The variables are overwritten at each attitude calculation invocation - the values |
28 | * The variables are overwritten at each attitude calculation invocation - the values |
29 | * are not preserved or reused. |
29 | * are not preserved or reused. |
30 | */ |
30 | */ |
31 | int16_t rate_ATT[3]; |
- | |
32 | - | ||
33 | // With different (less) filtering |
- | |
34 | int16_t rate_PID[3]; |
- | |
35 | int16_t differential[3]; |
- | |
36 | 31 | ||
37 | /* |
32 | /* |
38 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
33 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
39 | * horizontal plane, yaw relative to yaw at start. |
34 | * horizontal plane, yaw relative to yaw at start. |
40 | */ |
35 | */ |
41 | int32_t attitude[3]; |
36 | int32_t attitude[3]; |
42 | 37 | ||
43 | /* |
38 | /* |
44 | * Experiment: Compensating for dynamic-induced gyro biasing. |
39 | * Experiment: Compensating for dynamic-induced gyro biasing. |
45 | */ |
40 | */ |
46 | int16_t driftComp[3] = { 0, 0, 0 }; |
41 | int16_t driftComp[3] = { 0, 0, 0 }; |
47 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
42 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
48 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
43 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
49 | // int16_t dynamicCalCount; |
44 | // int16_t dynamicCalCount; |
50 | 45 | ||
51 | /************************************************************************ |
46 | /************************************************************************ |
52 | * Set inclination angles from the acc. sensor data. |
47 | * Set inclination angles from the acc. sensor data. |
53 | * If acc. sensors are not used, set to zero. |
48 | * If acc. sensors are not used, set to zero. |
54 | * TODO: One could use inverse sine to calculate the angles more |
49 | * TODO: One could use inverse sine to calculate the angles more |
55 | * accurately, but since: 1) the angles are rather small at times when |
50 | * accurately, but since: 1) the angles are rather small at times when |
56 | * it makes sense to set the integrals (standing on ground, or flying at |
51 | * it makes sense to set the integrals (standing on ground, or flying at |
57 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
52 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
58 | * it is hardly worth the trouble. |
53 | * it is hardly worth the trouble. |
59 | ************************************************************************/ |
54 | ************************************************************************/ |
60 | /* |
55 | /* |
61 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
56 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
62 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
57 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
63 | } |
58 | } |
64 | */ |
59 | */ |
65 | 60 | ||
66 | void setStaticAttitudeAngles(void) { |
61 | void setStaticAttitudeAngles(void) { |
67 | attitude[PITCH] = attitude[ROLL] = 0; |
62 | attitude[PITCH] = attitude[ROLL] = 0; |
68 | } |
63 | } |
69 | 64 | ||
70 | /************************************************************************ |
65 | /************************************************************************ |
71 | * Neutral Readings |
66 | * Neutral Readings |
72 | ************************************************************************/ |
67 | ************************************************************************/ |
73 | void attitude_setNeutral(void) { |
68 | void attitude_setNeutral(void) { |
74 | // Calibrate hardware. |
69 | // Calibrate hardware. |
75 | analog_setNeutral(); |
70 | analog_setNeutral(); |
76 | 71 | ||
77 | // reset gyro integrals to acc guessing |
72 | // reset gyro integrals to acc guessing |
78 | setStaticAttitudeAngles(); |
73 | setStaticAttitudeAngles(); |
79 | } |
74 | } |
80 | 75 | ||
81 | /************************************************************************ |
76 | /************************************************************************ |
82 | * Get sensor data from the analog module, and release the ADC |
77 | * Get sensor data from the analog module, and release the ADC |
83 | * TODO: Ultimately, the analog module could do this (instead of dumping |
78 | * TODO: Ultimately, the analog module could do this (instead of dumping |
84 | * the values into variables). |
79 | * the values into variables). |
85 | * The rate variable end up in a range of about [-1024, 1023]. |
80 | * The rate variable end up in a range of about [-1024, 1023]. |
86 | *************************************************************************/ |
81 | *************************************************************************/ |
87 | void getAnalogData(void) { |
82 | void getAnalogData(void) { |
88 | uint8_t axis; |
83 | uint8_t axis; |
89 | - | ||
90 | analog_update(); |
84 | analog_update(); |
91 | - | ||
92 | for (axis = PITCH; axis <= YAW; axis++) { |
85 | for (axis = PITCH; axis <= YAW; axis++) { |
93 | rate_PID[axis] = gyro_PID[axis]; |
- | |
94 | rate_ATT[axis] = gyro_ATT[axis]; |
- | |
95 | differential[axis] = gyroD[axis]; |
- | |
96 | } |
86 | } |
97 | } |
87 | } |
98 | 88 | ||
99 | void integrate(void) { |
89 | void integrate(void) { |
100 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
90 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
101 | uint8_t axis; |
91 | uint8_t axis; |
102 | 92 | ||
103 | /* |
93 | /* |
104 | * Yaw |
94 | * Yaw |
105 | * Calculate yaw gyro integral (~ to rotation angle) |
95 | * Calculate yaw gyro integral (~ to rotation angle) |
106 | * Limit heading proportional to 0 deg to 360 deg |
96 | * Limit heading proportional to 0 deg to 360 deg |
107 | */ |
97 | */ |
108 | /* |
98 | /* |
109 | * Pitch axis integration and range boundary wrap. |
99 | * Pitch axis integration and range boundary wrap. |
110 | */ |
100 | */ |
111 | 101 | ||
112 | for (axis = PITCH; axis <= YAW; axis++) { |
102 | for (axis = PITCH; axis <= YAW; axis++) { |
113 | attitude[axis] += rate_ATT[axis]; |
103 | attitude[axis] += gyro_ATT[axis]; |
114 | if (attitude[axis] > OVER180) { |
104 | if (attitude[axis] > OVER180) { |
115 | attitude[axis] -= OVER360; |
105 | attitude[axis] -= OVER360; |
116 | } else if (attitude[axis] <= -OVER180) { |
106 | } else if (attitude[axis] <= -OVER180) { |
117 | attitude[axis] += OVER360; |
107 | attitude[axis] += OVER360; |
118 | } |
108 | } |
119 | } |
109 | } |
120 | } |
110 | } |
121 | 111 | ||
122 | /************************************************************************ |
112 | /************************************************************************ |
123 | * Main procedure. |
113 | * Main procedure. |
124 | ************************************************************************/ |
114 | ************************************************************************/ |
125 | void calculateFlightAttitude(void) { |
115 | void calculateFlightAttitude(void) { |
126 | getAnalogData(); |
116 | getAnalogData(); |
127 | integrate(); |
117 | integrate(); |
128 | 118 | ||
129 | // We are done reading variables from the analog module. |
119 | // We are done reading variables from the analog module. |
130 | // Interrupt-driven sensor reading may restart. |
120 | // Interrupt-driven sensor reading may restart. |
131 | startAnalogConversionCycle(); |
121 | startAnalogConversionCycle(); |
132 | } |
122 | } |
133 | 123 |