Rev 1960 | Rev 1963 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1960 | Rev 1961 | ||
---|---|---|---|
Line 78... | Line 78... | ||
78 | /* |
78 | /* |
79 | * This could be expanded to take arguments from ohter sources than the RC |
79 | * This could be expanded to take arguments from ohter sources than the RC |
80 | * (read: Custom MK RC project) |
80 | * (read: Custom MK RC project) |
81 | */ |
81 | */ |
82 | uint8_t controlMixer_getArgument(void) { |
82 | uint8_t controlMixer_getArgument(void) { |
83 | return lastArgument; |
83 | return lastArgument; |
84 | } |
84 | } |
Line 85... | Line 85... | ||
85 | 85 | ||
86 | /* |
86 | /* |
87 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
87 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
88 | * than the R/C (read: Custom MK R/C project) |
88 | * than the R/C (read: Custom MK R/C project) |
89 | */ |
89 | */ |
90 | uint8_t controlMixer_getCommand(void) { |
90 | uint8_t controlMixer_getCommand(void) { |
91 | return lastCommand; |
91 | return lastCommand; |
Line 92... | Line 92... | ||
92 | } |
92 | } |
93 | 93 | ||
94 | uint8_t controlMixer_isCommandRepeated(void) { |
94 | uint8_t controlMixer_isCommandRepeated(void) { |
Line 95... | Line 95... | ||
95 | return isCommandRepeated; |
95 | return isCommandRepeated; |
- | 96 | } |
|
- | 97 | ||
- | 98 | void controlMixer_setNeutral() { |
|
96 | } |
99 | for (uint8_t i=0; i<VARIABLE_COUNT; i++) { |
97 | 100 | variables[i] = 0; |
|
98 | void controlMixer_setNeutral() { |
101 | } |
Line 99... | Line 102... | ||
99 | EC_setNeutral(); |
102 | EC_setNeutral(); |
100 | HC_setGround(); |
103 | HC_setGround(); |
101 | } |
104 | } |
102 | 105 | ||
103 | /* |
106 | /* |
104 | * Set the potientiometer values to the momentary values of the respective R/C channels. |
107 | * Set the potientiometer values to the momentary values of the respective R/C channels. |
105 | * No slew rate limitation. |
108 | * No slew rate limitation. |
106 | */ |
109 | */ |
107 | void controlMixer_initVariables(void) { |
110 | void controlMixer_initVariables(void) { |
108 | uint8_t i; |
111 | uint8_t i; |
Line 109... | Line 112... | ||
109 | for (i = 0; i < 8; i++) { |
112 | for (i=0; i < VARIABLE_COUNT; i++) { |
110 | variables[i] = RC_getVariable(i); |
113 | variables[i] = RC_getVariable(i); |
111 | } |
114 | } |
112 | } |
115 | } |
113 | 116 | ||
114 | /* |
117 | /* |
115 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
118 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
116 | * TODO: It assumes R/C as source. Not necessarily true. |
119 | * TODO: It assumes R/C as source. Not necessarily true. |
117 | */ |
120 | */ |
118 | void controlMixer_updateVariables(void) { |
121 | void controlMixer_updateVariables(void) { |
119 | uint8_t i; |
122 | uint8_t i; |
120 | int16_t targetvalue; |
123 | int16_t targetvalue; |
121 | for (i = 0; i < 8; i++) { |
124 | for (i=0; i < VARIABLE_COUNT; i++) { |
122 | targetvalue = RC_getVariable(i); |
125 | targetvalue = RC_getVariable(i); |
123 | if (targetvalue < 0) |
126 | if (targetvalue < 0) |
124 | targetvalue = 0; |
127 | targetvalue = 0; |
125 | if (variables[i] < targetvalue && variables[i] < 255) |
128 | if (variables[i] < targetvalue && variables[i] < 255) |
Line 126... | Line 129... | ||
126 | variables[i]++; |
129 | variables[i]++; |
127 | else if (variables[i] > 0 && variables[i] > targetvalue) |
130 | else if (variables[i] > 0 && variables[i] > targetvalue) |
128 | variables[i]--; |
131 | variables[i]--; |
129 | } |
132 | } |
130 | } |
133 | } |
131 | 134 | ||
Line 132... | Line 135... | ||
132 | uint8_t controlMixer_getSignalQuality(void) { |
135 | uint8_t controlMixer_getSignalQuality(void) { |
133 | uint8_t rcQ = RC_getSignalQuality(); |
136 | uint8_t rcQ = RC_getSignalQuality(); |
134 | uint8_t ecQ = EC_getSignalQuality(); |
137 | uint8_t ecQ = EC_getSignalQuality(); |
135 | // This needs not be the only correct solution... |
138 | // This needs not be the only correct solution... |
136 | return rcQ > ecQ ? rcQ : ecQ; |
139 | return rcQ > ecQ ? rcQ : ecQ; |
137 | } |
140 | } |
138 | 141 | ||
139 | void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
142 | void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
140 | int16_t tmp = controls[index]; |
143 | int16_t tmp = controls[index]; |
141 | controls[index] = newValue; |
144 | controls[index] = newValue; |
142 | tmp -= newValue; |
145 | tmp -= newValue; |
Line 143... | Line 146... | ||
143 | tmp /= 2; |
146 | tmp /= 2; |
144 | tmp = tmp * tmp; |
147 | tmp = tmp * tmp; |
145 | // tmp += (newValue >= 0) ? newValue : -newValue; |
148 | // tmp += (newValue >= 0) ? newValue : -newValue; |
146 | if (controlActivity + (uint16_t)tmp >= controlActivity) |
149 | if (controlActivity + (uint16_t)tmp >= controlActivity) |
147 | controlActivity += tmp; |
150 | controlActivity += tmp; |
148 | else controlActivity = 0xffff; |
151 | else controlActivity = 0xffff; |
149 | } |
152 | } |
Line 150... | Line 153... | ||
150 | 153 | ||
151 | #define CADAMPING 10 |
154 | #define CADAMPING 10 |
152 | void dampenControlActivity(void) { |
155 | void dampenControlActivity(void) { |
153 | uint32_t tmp = controlActivity; |
156 | uint32_t tmp = controlActivity; |
154 | tmp *= ((1<<CADAMPING)-1); |
157 | tmp *= ((1<<CADAMPING)-1); |
155 | tmp >>= CADAMPING; |
158 | tmp >>= CADAMPING; |
156 | controlActivity = tmp; |
159 | controlActivity = tmp; |
157 | } |
160 | } |
158 | 161 | ||
159 | /* |
162 | /* |
160 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
163 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
161 | */ |
164 | */ |
162 | void controlMixer_update(void) { |
165 | void controlMixer_update(void) { |
163 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
166 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
164 | // TODO: If no signal --> zero. |
167 | // TODO: If no signal --> zero. |
165 | int16_t tempThrottle; |
168 | int16_t tempThrottle; |
166 | 169 | ||
167 | // takes almost no time... |
170 | // takes almost no time... |
168 | RC_update(); |
171 | RC_update(); |
169 | 172 | ||
170 | // takes almost no time... |
173 | // takes almost no time... |
171 | EC_update(); |
174 | EC_update(); |
172 | 175 | ||
173 | // takes about 80 usec. |
176 | // takes about 80 usec. |
174 | HC_update(); |
177 | HC_update(); |
175 | 178 | ||
176 | int16_t* RC_PRTY = RC_getPRTY(); |
179 | int16_t* RC_PRTY = RC_getPRTY(); |
177 | int16_t* EC_PRTY = EC_getPRTY(); |
180 | int16_t* EC_PRTY = EC_getPRTY(); |
178 | 181 | ||
179 | updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
182 | updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
180 | updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
183 | updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
181 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
184 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
182 | dampenControlActivity(); |
185 | dampenControlActivity(); |
183 | 186 | ||
184 | //debugOut.analog[14] = controlActivity/10; |
187 | //debugOut.analog[14] = controlActivity/10; |
185 | 188 | ||
186 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
189 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
187 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
190 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
188 | 191 | ||
189 | // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
192 | // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
190 | 193 | ||
191 | if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
194 | if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
192 | controlMixer_updateVariables(); |
195 | controlMixer_updateVariables(); |
193 | configuration_applyVariablesToParams(); |
196 | configuration_applyVariablesToParams(); |
194 | //looping = RC_getLooping(looping); |
197 | //looping = RC_getLooping(looping); |
195 | } else { // Signal is not OK |
198 | } else { // Signal is not OK |
196 | // Could handle switch to emergency flight here. |
199 | // Could handle switch to emergency flight here. |
197 | // throttle is handled elsewhere. |
200 | // throttle is handled elsewhere. |
198 | // looping = 0; |
201 | // looping = 0; |
199 | } |
202 | } |
200 | 203 | ||
201 | // part1a end. |
204 | // part1a end. |
202 | 205 | ||
203 | /* This is not really necessary with the dead-band feature on all sticks (see rc.c) |
206 | /* This is not really necessary with the dead-band feature on all sticks (see rc.c) |
204 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
207 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
205 | if (controlYaw > 2) controlYaw-= 2; |
208 | if (controlYaw > 2) controlYaw-= 2; |
206 | else if (controlYaw< -2) controlYaw += 2; |
209 | else if (controlYaw< -2) controlYaw += 2; |
207 | else controlYaw = 0; |
210 | else controlYaw = 0; |
208 | } |
211 | } |
209 | */ |
212 | */ |
210 | 213 | ||
211 | /* |
214 | /* |
212 | * Record maxima |
215 | * Record maxima |
213 | for (axis = PITCH; axis <= ROLL; axis++) { |
216 | for (axis = PITCH; axis <= ROLL; axis++) { |
214 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
217 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
215 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
218 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
216 | if (maxControl[axis] > 100) |
219 | if (maxControl[axis] > 100) |
217 | maxControl[axis] = 100; |
220 | maxControl[axis] = 100; |
218 | } else if (maxControl[axis]) |
221 | } else if (maxControl[axis]) |
219 | maxControl[axis]--; |
222 | maxControl[axis]--; |
220 | } |
223 | } |
221 | */ |
224 | */ |
222 | 225 | ||
223 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
226 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
224 | : COMMAND_NONE; |
227 | : COMMAND_NONE; |
225 | uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
228 | uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
226 | : COMMAND_NONE; |
229 | : COMMAND_NONE; |
227 | 230 | ||
228 | if (rcCommand != COMMAND_NONE) { |
231 | if (rcCommand != COMMAND_NONE) { |
229 | isCommandRepeated = (lastCommand == rcCommand); |
232 | isCommandRepeated = (lastCommand == rcCommand); |
230 | lastCommand = rcCommand; |
233 | lastCommand = rcCommand; |
231 | lastArgument = RC_getArgument(); |
234 | lastArgument = RC_getArgument(); |
232 | } else if (ecCommand != COMMAND_NONE) { |
235 | } else if (ecCommand != COMMAND_NONE) { |
233 | isCommandRepeated = (lastCommand == ecCommand); |
236 | isCommandRepeated = (lastCommand == ecCommand); |
234 | lastCommand = ecCommand; |
237 | lastCommand = ecCommand; |
235 | lastArgument = EC_getArgument(); |
238 | lastArgument = EC_getArgument(); |
236 | } else { |
239 | } else { |
237 | // Both sources have no command, or one or both are out. |
240 | // Both sources have no command, or one or both are out. |
238 | // Just set to false. There is no reason to check if the none-command was repeated anyway. |
241 | // Just set to false. There is no reason to check if the none-command was repeated anyway. |
239 | isCommandRepeated = 0; |
242 | isCommandRepeated = 0; |
240 | lastCommand = COMMAND_NONE; |
243 | lastCommand = COMMAND_NONE; |
241 | } |
244 | } |
242 | 245 | ||
243 | /* |
246 | /* |
244 | if (isCommandRepeated) |
247 | if (isCommandRepeated) |
Line 245... | Line 248... | ||
245 | DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; |
248 | DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; |
246 | else |
249 | else |
247 | DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED; |
250 | DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED; |
248 | if (rcCommand) |
251 | if (rcCommand) |