Rev 1400 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1400 | Rev 1568 | ||
---|---|---|---|
Line -... | Line 1... | ||
- | 1 | ||
1 | #include "main.h" |
2 | #include "main.h" |
2 | #include "spectrum.h" |
3 | #include "spectrum.h" |
- | 4 | #include "heli.h" |
|
Line 3... | Line 5... | ||
3 | 5 | ||
4 | volatile unsigned int CountMilliseconds = 0; |
6 | volatile unsigned int CountMilliseconds = 0; |
5 | volatile static unsigned int tim_main; |
7 | volatile static unsigned int tim_main; |
6 | volatile unsigned char UpdateMotor = 0; |
8 | volatile unsigned char UpdateMotor = 0; |
7 | volatile unsigned int cntKompass = 0; |
9 | volatile unsigned int cntKompass = 0; |
8 | volatile unsigned int beeptime = 0; |
10 | volatile unsigned int beeptime = 0; |
Line 9... | Line 11... | ||
9 | volatile unsigned char SendSPI = 0, ServoActive = 0; |
11 | volatile unsigned char SendSPI = 0; |
Line 10... | Line 12... | ||
10 | 12 | ||
11 | unsigned int BeepMuster = 0xffff; |
13 | unsigned int BeepMuster = 0xffff; |
Line 214... | Line 216... | ||
214 | 216 | ||
215 | #define MULTIPLYER 4 |
217 | #define MULTIPLYER 4 |
216 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
218 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
Line 217... | Line -... | ||
217 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
- | |
218 | - | ||
219 | if(PlatinenVersion < 20) |
- | |
220 | { |
- | |
221 | //--------------------------- |
- | |
222 | // Nick servo state machine |
- | |
223 | //--------------------------- |
- | |
224 | if(!PulseOutput) // pulse output complete |
- | |
225 | { |
- | |
226 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
- | |
227 | { |
- | |
228 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
- | |
229 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
- | |
230 | - | ||
231 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
232 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
233 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
234 | { // inverting movement of servo |
- | |
235 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
236 | } |
- | |
237 | else |
- | |
238 | { // non inverting movement of servo |
- | |
239 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
240 | } |
- | |
241 | // limit servo value to its parameter range definition |
- | |
242 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
243 | { |
- | |
244 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
245 | } |
- | |
246 | else |
- | |
247 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
248 | { |
- | |
249 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
250 | } |
- | |
251 | - | ||
252 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
- | |
253 | - | ||
254 | ServoNickValue /= MULTIPLYER; |
- | |
255 | - | ||
256 | // range servo pulse width |
- | |
257 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
- | |
258 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
- | |
259 | // accumulate time for correct update rate |
- | |
260 | ServoFrameTime = RemainingPulse; |
- | |
261 | } |
- | |
262 | else // we had a high pulse |
- | |
263 | { |
- | |
264 | TCCR2A |= (1<<COM2A0); // make a low pulse |
- | |
265 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
- | |
266 | } |
- | |
267 | // set pulse output active |
- | |
268 | PulseOutput = 1; |
- | |
269 | } |
- | |
270 | } // EOF Nick servo state machine |
- | |
271 | else |
219 | static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
272 | { |
220 | |
273 | //----------------------------------------------------- |
221 | //----------------------------------------------------- |
274 | // PPM state machine, onboard demultiplexed by HEF4017 |
222 | // PPM state machine, onboard demultiplexed by HEF4017 |
275 | //----------------------------------------------------- |
223 | //----------------------------------------------------- |
Line 286... | Line 234... | ||
286 | HEF4017R_ON; // enable HEF4017 reset |
234 | HEF4017R_ON; // enable HEF4017 reset |
287 | } |
235 | } |
288 | else // servo channels |
236 | else // servo channels |
289 | { |
237 | { |
290 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
238 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
291 | switch(ServoIndex) // map servo channels |
- | |
292 | { |
- | |
293 | case 1: // Nick Compensation Servo |
- | |
294 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
295 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
296 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
297 | { // inverting movement of servo |
- | |
298 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
299 | } |
- | |
300 | else |
- | |
301 | { // non inverting movement of servo |
- | |
302 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
303 | } |
- | |
304 | // limit servo value to its parameter range definition |
- | |
305 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
306 | { |
- | |
307 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
308 | } |
- | |
309 | else |
- | |
310 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
311 | { |
- | |
312 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
313 | } |
- | |
314 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
- | |
315 | ServoNickValue /= MULTIPLYER; |
- | |
316 | break; |
- | |
317 | case 2: // Roll Compensation Servo |
- | |
318 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
- | |
319 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
320 | if(EE_Parameter.ServoCompInvert & 0x02) |
- | |
321 | { // inverting movement of servo |
- | |
322 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
323 | } |
- | |
324 | else |
- | |
325 | { // non inverting movement of servo |
- | |
326 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
327 | } |
- | |
328 | // limit servo value to its parameter range definition |
- | |
329 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
- | |
330 | { |
- | |
331 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
- | |
332 | } |
- | |
333 | else |
- | |
334 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
- | |
335 | { |
- | |
336 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
- | |
337 | } |
- | |
338 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
239 | RemainingPulse += 2 * servoValues[ServoIndex - 1]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
339 | ServoRollValue /= MULTIPLYER; |
- | |
340 | //DebugOut.Analog[20] = ServoRollValue; |
- | |
341 | break; |
- | |
342 | 240 | ||
343 | default: // other servo channels |
- | |
344 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
- | |
345 | break; |
- | |
346 | } |
- | |
347 | // range servo pulse width |
241 | // range servo pulse width |
348 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
242 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
349 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
243 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
350 | // substract stop pulse width |
244 | // substract stop pulse width |
351 | RemainingPulse -= PPM_STOPPULSE; |
245 | RemainingPulse -= PPM_STOPPULSE; |
Line 358... | Line 252... | ||
358 | TCCR2A |= (1<<COM2A0); // make a low pulse |
252 | TCCR2A |= (1<<COM2A0); // make a low pulse |
359 | // set pulsewidth to stop pulse width |
253 | // set pulsewidth to stop pulse width |
360 | RemainingPulse = PPM_STOPPULSE; |
254 | RemainingPulse = PPM_STOPPULSE; |
361 | // accumulate time for correct sync gap |
255 | // accumulate time for correct sync gap |
362 | ServoFrameTime += RemainingPulse; |
256 | ServoFrameTime += RemainingPulse; |
363 | if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset |
257 | HEF4017R_OFF; // disable HEF4017 reset |
364 | else HEF4017R_ON; |
- | |
365 | ServoIndex++; // change to next servo channel |
258 | ServoIndex++; // change to next servo channel |
366 | if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
259 | if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
367 | } |
260 | } |
368 | // set pulse output active |
261 | // set pulse output active |
369 | PulseOutput = 1; |
262 | PulseOutput = 1; |
370 | } |
263 | } |
371 | } // EOF PPM state machine |
- | |
Line 372... | Line 264... | ||
372 | 264 | ||
373 | // General pulse output generator |
265 | // General pulse output generator |
374 | if(RemainingPulse > (255 + IRS_RUNTIME)) |
266 | if(RemainingPulse > (255 + IRS_RUNTIME)) |
375 | { |
267 | { |