Rev 1612 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1612 | Rev 1623 | ||
|---|---|---|---|
| Line 46... | Line 46... | ||
| 46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 49 | // + POSSIBILITY OF SUCH DAMAGE. |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
| 50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 51 | ||
| - | 52 | /* |
|
| - | 53 | OBSOLETED BY controlMixer.c. But this is how it looked - maybe somebody will find it simpler? |
|
| - | 54 | ||
| 51 | #include <stdlib.h> |
55 | #include <stdlib.h> |
| 52 | #include "control.h" |
56 | #include "control.h" |
| Line 53... | Line 57... | ||
| 53 | 57 | ||
| 54 | #include "rc.h" |
58 | #include "rc.h" |
| Line 75... | Line 79... | ||
| 75 | uint8_t lastStickCommand = STICK_COMMAND_UNDEF; |
79 | uint8_t lastStickCommand = STICK_COMMAND_UNDEF; |
| 76 | uint8_t stickCommandTimer = 0; |
80 | uint8_t stickCommandTimer = 0; |
| Line 77... | Line 81... | ||
| 77 | 81 | ||
| Line 78... | Line 82... | ||
| 78 | ExternalControl_t externalControl; |
82 | ExternalControl_t externalControl; |
| 79 | 83 | ||
| 80 | /* |
84 | / * |
| 81 | * Stick diagram: |
85 | * Stick diagram: |
| 82 | * 2--3--4 |
86 | * 2--3--4 |
| 83 | * | | + |
87 | * | | + |
| Line 87... | Line 91... | ||
| 87 | * |
91 | * |
| 88 | * + <-- |
92 | * + <-- |
| 89 | * 0 |
93 | * 0 |
| 90 | * |
94 | * |
| 91 | * Not in any of these positions: 0 |
95 | * Not in any of these positions: 0 |
| 92 | */ |
96 | * / |
| Line 93... | Line 97... | ||
| 93 | 97 | ||
| 94 | /* |
98 | / * |
| 95 | * The stick most be further from center than this to indicate a settings number (1-5). |
99 | * The stick most be further from center than this to indicate a settings number (1-5). |
| 96 | */ |
100 | * / |
| Line 97... | Line 101... | ||
| 97 | #define STICK_SETTINGSELECTION_THRESHOLD 70 |
101 | #define STICK_SETTINGSELECTION_THRESHOLD 70 |
| 98 | 102 | ||
| 99 | uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) { |
103 | uint8_t control_getLeftRCStickIndex(int16_t thresholdThrottle, int16_t thresholdYaw) { |
| Line 140... | Line 144... | ||
| 140 | return 5; |
144 | return 5; |
| 141 | return 9; |
145 | return 9; |
| 142 | } |
146 | } |
| 143 | } |
147 | } |
| Line 144... | Line 148... | ||
| 144 | 148 | ||
| 145 | /* |
149 | / * |
| 146 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
150 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
| 147 | * than the R/C (read: Custom MK R/C project) |
151 | * than the R/C (read: Custom MK R/C project) |
| 148 | */ |
152 | * / |
| 149 | void control_senseStickCommands(void) { |
153 | void control_senseStickCommands(void) { |
| 150 | uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85); |
154 | uint8_t stickCommandNow = control_getLeftRCStickIndex(85, 85); |
| 151 | if (stickCommandNow != lastStickCommand) { |
155 | if (stickCommandNow != lastStickCommand) { |
| 152 | lastStickCommand = stickCommandNow; |
156 | lastStickCommand = stickCommandNow; |
| Line 155... | Line 159... | ||
| 155 | if (stickCommandTimer < 201) |
159 | if (stickCommandTimer < 201) |
| 156 | stickCommandTimer++; |
160 | stickCommandTimer++; |
| 157 | } |
161 | } |
| 158 | } |
162 | } |
| Line 159... | Line 163... | ||
| 159 | 163 | ||
| 160 | /* |
164 | / * |
| 161 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
165 | * This could be expanded to take calibrate / start / stop commands from ohter sources |
| 162 | * than the R/C (read: Custom MK R/C project) |
166 | * than the R/C (read: Custom MK R/C project) |
| 163 | */ |
167 | * / |
| 164 | uint8_t control_getStickCommand(void) { |
168 | uint8_t control_getStickCommand(void) { |
| 165 | // If the same command was made 200 times, it's stable. |
169 | // If the same command was made 200 times, it's stable. |
| 166 | if (stickCommandTimer >= 200) { |
170 | if (stickCommandTimer >= 200) { |
| 167 | return lastStickCommand; |
171 | return lastStickCommand; |
| Line 171... | Line 175... | ||
| 171 | 175 | ||
| 172 | uint8_t control_isStickCommandRepeated(void) { |
176 | uint8_t control_isStickCommandRepeated(void) { |
| 173 | return stickCommandTimer > 200 ? 1 : 0; |
177 | return stickCommandTimer > 200 ? 1 : 0; |
| Line 174... | Line 178... | ||
| 174 | } |
178 | } |
| 175 | 179 | ||
| 176 | /* |
180 | / * |
| 177 | * To be fired only when the right stick is in the center position. |
181 | * To be fired only when the right stick is in the center position. |
| 178 | * This will cause the value of pitch and roll stick to be adjusted |
182 | * This will cause the value of pitch and roll stick to be adjusted |
| 179 | * to zero (not just to near zero, as per the assumption in rc.c |
183 | * to zero (not just to near zero, as per the assumption in rc.c |
| 180 | * about the rc signal. I had values about 50..70 with a Futaba |
184 | * about the rc signal. I had values about 50..70 with a Futaba |
| 181 | * R617 receiver.) This calibration is not strictly necessary, but |
185 | * R617 receiver.) This calibration is not strictly necessary, but |
| 182 | * for control logic that depends on the exact (non)center position |
186 | * for control logic that depends on the exact (non)center position |
| 183 | * of a stick, it may be useful. |
187 | * of a stick, it may be useful. |
| 184 | */ |
188 | * / |
| 185 | void control_setNeutral(void) { |
189 | void control_setNeutral(void) { |
| 186 | stickOffsetPitch += stickPitch; |
190 | stickOffsetPitch += stickPitch; |
| Line 187... | Line 191... | ||
| 187 | stickOffsetRoll += stickRoll; |
191 | stickOffsetRoll += stickRoll; |
| 188 | } |
192 | } |
| 189 | 193 | ||
| 190 | /* |
194 | / * |
| 191 | * Set the potientiometer values to the values of the respective R/C channel |
195 | * Set the potientiometer values to the values of the respective R/C channel |
| 192 | * right now. No slew rate limit. |
196 | * right now. No slew rate limit. |
| 193 | */ |
197 | * / |
| 194 | void control_initPots(void) { |
198 | void control_initPots(void) { |
| 195 | uint8_t i; |
199 | uint8_t i; |
| 196 | for (i=0; i<4; i++) { |
200 | for (i=0; i<4; i++) { |
| 197 | pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET; |
201 | pots[i] = RCChannel(CH_POTS + i) + POT_OFFSET; |
| 198 | } |
202 | } |
| 199 | for (i=4; i<8; i++) { |
203 | for (i=4; i<8; i++) { |
| Line 200... | Line 204... | ||
| 200 | pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET; |
204 | pots[i] = PPM_in[9 + (i-4)] + POT_OFFSET; |
| 201 | } |
205 | } |
| 202 | } |
206 | } |
| 203 | 207 | ||
| 204 | /* |
208 | / * |
| 205 | * Update potentiometer values with slow slew rate. Could be made faster if desired. |
209 | * Update potentiometer values with slow slew rate. Could be made faster if desired. |
| 206 | */ |
210 | * / |
| 207 | void control_updatePots(void) { |
211 | void control_updatePots(void) { |
| Line 215... | Line 219... | ||
| 215 | if (targetvalue < 0) targetvalue = 0; |
219 | if (targetvalue < 0) targetvalue = 0; |
| 216 | if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--; |
220 | if (pots[i] < targetvalue && pots[i] < 255) pots[i]++; else if(pots[i] > 0 && pots[i] > targetvalue) pots[i]--; |
| 217 | } |
221 | } |
| 218 | } |
222 | } |
| Line 219... | Line 223... | ||
| 219 | 223 | ||
| 220 | /* |
224 | / * |
| 221 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
225 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| 222 | */ |
226 | * / |
| 223 | void control_update(void) { |
227 | void control_update(void) { |
| 224 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
228 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| 225 | stickPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
229 | stickPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
| 226 | // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4; |
230 | // (stick_pitch * 3 + RCChannel(CH_PITCH) * staticParams.StickP) / 4; |
| Line 313... | Line 317... | ||
| 313 | if(compassCalState < 5) beepNumber(compassCalState); |
317 | if(compassCalState < 5) beepNumber(compassCalState); |
| 314 | else beep(1000); |
318 | else beep(1000); |
| 315 | } |
319 | } |
| 316 | } |
320 | } |
| Line 317... | Line 321... | ||
| 317 | 321 | ||
| 318 | /* |
322 | / * |
| 319 | * |
323 | * |
| 320 | */ |
324 | * / |
| 321 | uint8_t control_hasNewRCData(void) { |
325 | uint8_t control_hasNewRCData(void) { |
| 322 | // return !NewPpmData--; |
326 | // return !NewPpmData--; |
| 323 | return (NewPpmData-- == 0) ? 1 : 0; |
327 | return (NewPpmData-- == 0) ? 1 : 0; |
| Line 367... | Line 371... | ||
| 367 | control_setNeutral(); // Calibrate right stick neutral position. |
371 | control_setNeutral(); // Calibrate right stick neutral position. |
| 368 | beepNumber(getActiveParamSet()); |
372 | beepNumber(getActiveParamSet()); |
| 369 | } |
373 | } |
| 370 | } |
374 | } |
| 371 | } // end !MOTOR_RUN condition. |
375 | } // end !MOTOR_RUN condition. |
| - | 376 | /* |
|