Rev 2058 | Rev 2062 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2058 | Rev 2059 | ||
---|---|---|---|
Line 54... | Line 54... | ||
54 | #include <string.h> |
54 | #include <string.h> |
55 | #include "configuration.h" |
55 | #include "configuration.h" |
56 | #include "sensors.h" |
56 | #include "sensors.h" |
57 | #include "rc.h" |
57 | #include "rc.h" |
58 | #include "output.h" |
58 | #include "output.h" |
- | 59 | #include "flight.h" |
|
Line 59... | Line 60... | ||
59 | 60 | ||
60 | int16_t variables[VARIABLE_COUNT]; |
61 | int16_t variables[VARIABLE_COUNT]; |
61 | ParamSet_t staticParams; |
62 | ParamSet_t staticParams; |
62 | channelMap_t channelMap; |
63 | channelMap_t channelMap; |
63 | mixerMatrix_t mixerMatrix; |
64 | mixerMatrix_t mixerMatrix; |
Line 64... | Line 65... | ||
64 | volatile dynamicParam_t dynamicParams; |
65 | volatile DynamicParams_t dynamicParams; |
65 | 66 | ||
66 | uint8_t CPUType = ATMEGA644; |
67 | uint8_t CPUType = ATMEGA644; |
Line 71... | Line 72... | ||
71 | 72 | ||
72 | // MK flags. TODO: Replace by enum. State machine. |
73 | // MK flags. TODO: Replace by enum. State machine. |
73 | uint16_t isFlying = 0; |
74 | uint16_t isFlying = 0; |
Line 74... | Line -... | ||
74 | volatile uint8_t MKFlags = 0; |
- | |
75 | 75 | volatile uint8_t MKFlags = 0; |
|
76 | /************************************************************************ |
76 | |
77 | * Map the parameter to pot values |
- | |
78 | * Replacing this code by the code below saved almost 1 kbyte. |
- | |
79 | ************************************************************************/ |
- | |
80 | - | ||
81 | void configuration_applyVariablesToParams(void) { |
- | |
82 | uint8_t i; |
- | |
83 | 77 | const MMXLATION XLATIONS[] = { |
|
84 | #define SET_POT_MM(b,a,min,max) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a; if(b<=min) b=min; else if(b>=max) b=max;} |
78 | {offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255}, |
85 | #define SET_POT(b,a) {if (a>=255-VARIABLE_COUNT) b=variables[a+VARIABLE_COUNT-255]; else b=a;} |
79 | {offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255}, |
86 | SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200); |
80 | {offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255}, |
87 | SET_POT(dynamicParams.gyroI, staticParams.gyroI); |
81 | {offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255}, |
88 | SET_POT(dynamicParams.gyroD, staticParams.gyroD); |
- | |
89 | //SET_POT(dynamicParams.compassControlHeading,staticParams.compassControlHeading); |
82 | {offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255}, |
90 | 83 | {offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255}, |
|
91 | SET_POT(dynamicParams.externalControl, staticParams.externalControl); |
- | |
92 | SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability); |
- | |
93 | SET_POT(dynamicParams.maxAccVector,staticParams.maxAccVector); |
84 | {offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
94 | 85 | {offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255}, |
|
95 | SET_POT(dynamicParams.heightP, staticParams.heightP); |
86 | {offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255}, |
96 | SET_POT(dynamicParams.heightI, staticParams.heightI); |
87 | {offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255}, |
97 | SET_POT(dynamicParams.heightD, staticParams.heightD); |
- | |
98 | SET_POT(dynamicParams.heightSetting,staticParams.heightSetting); |
- | |
99 | - | ||
100 | SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl); |
88 | {offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255}, |
101 | 89 | {offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255}, |
|
102 | SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl); |
- | |
103 | SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl); |
90 | {offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
104 | - | ||
105 | SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255); |
- | |
106 | SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255); |
- | |
107 | - | ||
108 | SET_POT(dynamicParams.levelCorrection[0], staticParams.levelCorrection[0]); |
- | |
109 | SET_POT(dynamicParams.levelCorrection[1], staticParams.levelCorrection[1]); |
- | |
110 | - | ||
111 | SET_POT(dynamicParams.naviMode, staticParams.naviMode); |
- | |
112 | - | ||
113 | for (i=0; i<sizeof(staticParams.userParams); i++) { |
- | |
114 | SET_POT(dynamicParams.userParams[i],staticParams.userParams[i]); |
- | |
115 | } |
- | |
116 | } |
- | |
117 | 91 | {offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
|
118 | const XLATION XLATIONS[] = { |
- | |
119 | {offsetof(ParamSet_t, heightSetting), offsetof(dynamicParam_t, heightSetting)}, |
- | |
120 | }; |
- | |
121 | 92 | {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
|
122 | const MMXLATION MMXLATIONS[] = { |
- | |
Line 123... | Line 93... | ||
123 | {offsetof(ParamSet_t, heightD), offsetof(dynamicParam_t, heightD),0,100}, |
93 | {offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
124 | }; |
94 | {offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}}; |
125 | 95 | ||
126 | uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
96 | uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
127 | uint8_t result; |
97 | uint8_t result; |
128 | if (src>=251) result = variables[src-251]; |
98 | if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
129 | else result = src; |
99 | else result = src; |
130 | if (result < min) result = min; |
100 | if (result < min) result = min; |
Line 131... | Line 101... | ||
131 | else if (result > max) result = max; |
101 | else if (result > max) result = max; |
132 | return result; |
102 | return result; |
133 | } |
103 | } |
134 | - | ||
135 | void configuration_applyVariablesToParams_dead(void) { |
- | |
136 | uint8_t i, src; |
- | |
137 | uint8_t* pointerToTgt; |
- | |
138 | for(i=0; i<sizeof(XLATIONS)/sizeof(XLATION); i++) { |
- | |
139 | src = *((uint8_t*)(&staticParams + XLATIONS[i].sourceIdx)); |
- | |
140 | pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
- | |
Line 141... | Line 104... | ||
141 | if (src < 255) { |
104 | |
142 | *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
105 | void configuration_applyVariablesToParams(void) { |
143 | } |
106 | uint8_t i, src; |
144 | } |
- | |
145 | 107 | uint8_t* pointerToTgt; |
|
146 | for(i=0; i<sizeof(MMXLATIONS)/sizeof(MMXLATION); i++) { |
- | |
147 | src = *((uint8_t*)(&staticParams + MMXLATIONS[i].sourceIdx)); |
108 | |
148 | pointerToTgt = (uint8_t*)(&dynamicParams + XLATIONS[i].targetIdx); |
109 | for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
- | 110 | src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
|
149 | if (src < 255) { |
111 | pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
150 | *pointerToTgt = configuration_applyVariableToParam(src, MMXLATIONS[i].min, MMXLATIONS[i].max); |
112 | *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
151 | } |
113 | } |
152 | } |
- | |
153 | 114 | ||
154 | for (i=0; i<sizeof(staticParams.userParams); i++) { |
- | |
155 | src = *((uint8_t*)(&staticParams + offsetof(ParamSet_t, userParams) + i)); |
115 | // User parameters are always variable. |
156 | pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i); |
116 | for (i=0; i<sizeof(staticParams.userParams); i++) { |
Line 157... | Line 117... | ||
157 | if (src < 255) { |
117 | src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
158 | *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
118 | pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
Line 174... | Line 134... | ||
174 | * from the makefile. |
134 | * from the makefile. |
175 | * However - we still do detect the board release. Reason: Otherwise it would be too |
135 | * However - we still do detect the board release. Reason: Otherwise it would be too |
176 | * tedious to have to modify the code for how to turn on and off LEDs when deploying |
136 | * tedious to have to modify the code for how to turn on and off LEDs when deploying |
177 | * on different HW version.... |
137 | * on different HW version.... |
178 | */ |
138 | */ |
179 | - | ||
180 | uint8_t getBoardRelease(void) { |
139 | uint8_t getBoardRelease(void) { |
181 | uint8_t boardRelease = 13; |
140 | uint8_t boardRelease = 13; |
182 | // the board release is coded via the pull up or down the 2 status LED |
141 | // the board release is coded via the pull up or down the 2 status LED |
Line 183... | Line 142... | ||
183 | 142 | ||
Line 207... | Line 166... | ||
207 | RED_OFF; |
166 | RED_OFF; |
208 | GRN_OFF; |
167 | GRN_OFF; |
209 | return boardRelease; |
168 | return boardRelease; |
210 | } |
169 | } |
Line -... | Line 170... | ||
- | 170 | ||
- | 171 | void configuration_setNormalFlightParameters(void) { |
|
- | 172 | flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
|
- | 173 | staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
|
- | 174 | dynamicParams.gyroP, staticParams.yawIFactor); |
|
- | 175 | } |
|
- | 176 | ||
- | 177 | void configuration_setFailsafeFlightParameters(void) { |
|
- | 178 | flight_setParameters(0, 90, 120, 90, 120); |
|
- | 179 | } |
|
- | 180 | ||
- | 181 | // Called after a change in configuration parameters, as a hook for modules to take over changes. |
|
- | 182 | void configuration_paramSetDidChange(void) { |
|
- | 183 | // This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
|
- | 184 | configuration_setNormalFlightParameters(); |
|
- | 185 | // Immediately load changes to output, and also signal the paramset change. |
|
- | 186 | output_init(); |
|
- | 187 | } |
|
211 | 188 | ||
212 | void setOtherDefaults(void) { |
189 | void setOtherDefaults(void) { |
213 | // Height Control |
190 | // Height Control |
214 | staticParams.airpressureFilterConstant = 8; |
191 | staticParams.airpressureFilterConstant = 8; |
215 | //staticParams.airpressureWindowLength = 0; |
192 | //staticParams.airpressureWindowLength = 0; |
Line 244... | Line 221... | ||
244 | // staticParams.zerothOrderCorrection = |
221 | // staticParams.zerothOrderCorrection = |
245 | // staticParams.driftCompDivider = |
222 | // staticParams.driftCompDivider = |
246 | // staticParams.driftCompLimit = |
223 | // staticParams.driftCompLimit = |
Line 247... | Line 224... | ||
247 | 224 | ||
248 | staticParams.dynamicStability = 50; |
225 | staticParams.dynamicStability = 50; |
- | 226 | staticParams.zerothOrderCorrectionAccTolerance = 60; |
|
249 | staticParams.maxAccVector = 10; |
227 | staticParams.zerothOrderCorrectionControlTolerance = 60; |
250 | staticParams.IFactor = 52; |
228 | staticParams.IFactor = 52; |
251 | staticParams.yawIFactor = 100; |
229 | staticParams.yawIFactor = 100; |
252 | staticParams.compassYawCorrection = 64; |
230 | staticParams.compassYawCorrection = 64; |
253 | staticParams.compassP = 50; |
231 | staticParams.compassP = 50; |