Rev 1645 | Rev 1796 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1645 | Rev 1775 | ||
---|---|---|---|
Line 50... | Line 50... | ||
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 51... | Line 51... | ||
51 | 51 | ||
52 | #include <stdlib.h> |
52 | #include <stdlib.h> |
53 | #include "controlMixer.h" |
53 | #include "controlMixer.h" |
- | 54 | #include "rc.h" |
|
- | 55 | #include "heightControl.h" |
|
54 | #include "rc.h" |
56 | #include "attitudeControl.h" |
55 | #include "externalControl.h" |
57 | #include "externalControl.h" |
56 | #include "configuration.h" |
58 | #include "configuration.h" |
57 | #include "attitude.h" |
59 | #include "attitude.h" |
58 | #include "eeprom.h" |
60 | #include "commands.h" |
59 | #include "flight.h" |
- | |
60 | - | ||
61 | /* |
- | |
62 | * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC. |
- | |
63 | */ |
- | |
Line 64... | Line 61... | ||
64 | #define COMMAND_TIMER 200 |
61 | #include "output.h" |
- | 62 | ||
65 | 63 | uint16_t maxControl[2] = {0,0}; |
|
66 | uint16_t maxControl[2] = {0,0}; |
64 | int16_t control[2] = {0,0}; |
Line 67... | Line 65... | ||
67 | int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0; |
65 | int16_t controlYaw = 0, controlThrottle = 0; |
68 | uint8_t looping = 0; |
66 | uint8_t looping = 0; |
- | 67 | ||
- | 68 | // Internal variables for reading commands made with an R/C stick. |
|
69 | 69 | uint8_t lastCommand = COMMAND_NONE; |
|
- | 70 | uint8_t lastArgument; |
|
- | 71 | ||
- | 72 | uint8_t isCommandRepeated = 0; |
|
- | 73 | ||
Line 70... | Line 74... | ||
70 | // Internal variables for reading commands made with an R/C stick. |
74 | // MK flags. TODO: Replace by enum. State machine. |
71 | uint8_t lastCommand = COMMAND_NONE; |
75 | uint16_t isFlying = 0; |
72 | uint8_t commandTimer = 0; |
76 | volatile uint8_t MKFlags = 0; |
73 | 77 | ||
74 | /* |
78 | /* |
75 | * 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 |
76 | * (read: Custom MK RC project) |
80 | * (read: Custom MK RC project) |
Line 77... | Line 81... | ||
77 | */ |
81 | */ |
78 | uint8_t controlMixer_getArgument(void) { |
82 | uint8_t controlMixer_getArgument(void) { |
79 | return RC_getArgument(); |
83 | return lastArgument; |
80 | } |
84 | } |
81 | 85 | ||
82 | /* |
- | |
83 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
- | |
84 | * than the R/C (read: Custom MK R/C project) |
86 | /* |
85 | */ |
- | |
86 | uint8_t controlMixer_getCommand(void) { |
- | |
87 | // If the same command was made COMMAND_TIMER times, it's stable. |
87 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
Line 88... | Line 88... | ||
88 | if (commandTimer >= COMMAND_TIMER) { |
88 | * than the R/C (read: Custom MK R/C project) |
89 | return lastCommand; |
89 | */ |
90 | } |
90 | uint8_t controlMixer_getCommand(void) { |
Line 91... | Line 91... | ||
91 | return COMMAND_NONE; |
91 | return lastCommand; |
92 | } |
- | |
93 | 92 | } |
|
94 | uint8_t controlMixer_isCommandRepeated(void) { |
93 | |
95 | return commandTimer > COMMAND_TIMER ? 1 : 0; |
94 | uint8_t controlMixer_isCommandRepeated(void) { |
Line 96... | Line 95... | ||
96 | } |
95 | return isCommandRepeated; |
97 | 96 | } |
|
98 | void controlMixer_setNeutral(uint8_t calibrate) { |
97 | |
Line 112... | Line 111... | ||
112 | } |
111 | } |
113 | } |
112 | } |
Line 114... | Line 113... | ||
114 | 113 | ||
115 | /* |
114 | /* |
- | 115 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
|
116 | * Update potentiometer values with limited slew rate. Could be made faster if desired. |
116 | * TODO: It assumes R/C as source. Not necessarily true. |
117 | */ |
117 | */ |
118 | void controlMixer_updateVariables(void) { |
118 | void controlMixer_updateVariables(void) { |
119 | uint8_t i; |
119 | uint8_t i; |
120 | int16_t targetvalue; |
120 | int16_t targetvalue; |
Line 137... | Line 137... | ||
137 | */ |
137 | */ |
138 | void controlMixer_update(void) { |
138 | void controlMixer_update(void) { |
139 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
139 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
140 | // TODO: If no signal --> zero. |
140 | // TODO: If no signal --> zero. |
141 | uint8_t axis; |
141 | uint8_t axis; |
- | 142 | ||
- | 143 | // takes almost no time... |
|
142 | RC_update(); |
144 | RC_update(); |
- | 145 | ||
- | 146 | // takes almost no time... |
|
143 | EC_update(); |
147 | EC_update(); |
Line -... | Line 148... | ||
- | 148 | ||
- | 149 | // takes about 80 usec. |
|
- | 150 | HC_update(); |
|
144 | 151 | ||
145 | int16_t* RC_PRTY = RC_getPRTY(); |
152 | int16_t* RC_PRTY = RC_getPRTY(); |
Line 146... | Line 153... | ||
146 | int16_t* EC_PRTY = EC_getPRTY(); |
153 | int16_t* EC_PRTY = EC_getPRTY(); |
147 | 154 | ||
- | 155 | control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
|
148 | control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
156 | control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
149 | control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
157 | // This can be a CPU time killer if the function implementations are inefficient. |
Line -... | Line 158... | ||
- | 158 | controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE])); |
|
- | 159 | controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
|
- | 160 | ||
- | 161 | DebugOut.Analog[12] = control[PITCH]; |
|
150 | controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]; |
162 | DebugOut.Analog[13] = control[ROLL]; |
151 | controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
163 | //DebugOut.Analog[26] = controlYaw; |
152 | 164 | ||
153 | if (RC_getSignalQuality() >= SIGNAL_GOOD) { |
165 | if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
154 | controlMixer_updateVariables(); |
166 | controlMixer_updateVariables(); |
155 | configuration_applyVariablesToParams(); |
167 | configuration_applyVariablesToParams(); |
156 | looping = RC_getLooping(looping); |
168 | looping = RC_getLooping(looping); |
157 | } else { // Signal is not OK |
169 | } else { // Signal is not OK |
158 | // Could handle switch to emergency flight here. |
170 | // Could handle switch to emergency flight here. |
Line 159... | Line 171... | ||
159 | // throttle is handled elsewhere. |
171 | // throttle is handled elsewhere. |
Line 160... | Line -... | ||
160 | looping = 0; |
- | |
161 | } |
172 | looping = 0; |
162 | 173 | } |
|
163 | DebugOut.Analog[18] = (int16_t)looping; |
174 | |
164 | 175 | // part1a end. |
|
165 | // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction) |
176 | |
166 | // TODO: Correct, for changed range (stick gain + expo is now applied already) |
177 | /* This is not really necessary with the pull-towards-zero of all sticks (see rc.c) |
- | 178 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
|
Line 167... | Line 179... | ||
167 | if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
179 | if (controlYaw > 2) controlYaw-= 2; |
168 | if (controlYaw > 2) controlYaw-= 2; |
180 | else if (controlYaw< -2) controlYaw += 2; |
169 | else if (controlYaw< -2) controlYaw += 2; |
181 | else controlYaw = 0; |
170 | else controlYaw = 0; |
182 | } |
Line 178... | Line 190... | ||
178 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
190 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
179 | if(maxControl[axis] > 100) maxControl[axis] = 100; |
191 | if(maxControl[axis] > 100) maxControl[axis] = 100; |
180 | } else if (maxControl[axis]) maxControl[axis]--; |
192 | } else if (maxControl[axis]) maxControl[axis]--; |
181 | } |
193 | } |
Line 182... | Line 194... | ||
182 | 194 | ||
183 | // Here we could blend in other sources. |
195 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
184 | uint8_t commandNow = RC_getCommand(); |
196 | uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
185 | 197 | ||
- | 198 | if (rcCommand != COMMAND_NONE) { |
|
186 | if (commandNow != lastCommand) { |
199 | isCommandRepeated = (lastCommand == rcCommand); |
187 | lastCommand = commandNow; |
200 | lastCommand = rcCommand; |
188 | commandTimer = 0; |
201 | lastArgument = RC_getArgument(); |
- | 202 | } else if (ecCommand != COMMAND_NONE) { |
|
- | 203 | isCommandRepeated = (lastCommand == ecCommand); |
|
- | 204 | lastCommand = ecCommand; |
|
- | 205 | lastArgument = EC_getArgument(); |
|
- | 206 | } else { |
|
- | 207 | // Both sources have no command, or one or both are out. |
|
189 | } else if (commandTimer < COMMAND_TIMER + 1) |
208 | // Just set to false. There is no reason to check if the none-command was repeated anyway. |
- | 209 | isCommandRepeated = 0; |
|
190 | commandTimer++; |
210 | lastCommand = COMMAND_NONE; |
191 | } |
211 | } |
- | 212 | ||
- | 213 | if (isCommandRepeated) DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED; |
|
192 | 214 | if (rcCommand) DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED; |
|
193 | /* |
- | |
194 | void setCompassCalState(void) { |
215 | |
195 | static uint8_t stick = 1; |
- | |
196 | // if pitch is centered or top set stick to zero |
- | |
- | 216 | // part1 end. |
|
- | 217 | } |
|
197 | if(RCChannel(CH_PITCH) > -20) stick = 0; |
218 | |
198 | // if pitch is down trigger to next cal state |
219 | // TODO: Integrate into command system. |
199 | if((RCChannel(CH_PITCH) < -70) && !stick) { |
- | |
200 | stick = 1; |
220 | uint8_t controlMixer_testCompassCalState(void) { |
201 | compassCalState++; |
- | |
202 | if(compassCalState < 5) beepNumber(compassCalState); |
- | |
203 | else beep(1000); |
- | |
204 | } |
221 | return RC_testCompassCalState(); |
205 | } |
- |