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