Subversion Repositories FlightCtrl

Rev

Rev 1612 | Rev 1621 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1612 Rev 1615
Line 1... Line 1...
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + Nur für den privaten Gebrauch
3
// + Nur f�r den privaten Gebrauch
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
6
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
7
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
11
// + Verkauf von Luftbildaufnahmen, usw.
11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
17
// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
18
// + eindeutig als Ursprung verlinkt werden
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
20
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
21
// + Benutzung auf eigene Gefahr
21
// + Benutzung auf eigene Gefahr
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
22
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25
// + mit unserer Zustimmung zulässig
25
// + mit unserer Zustimmung zul�ssig
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30
// + this list of conditions and the following disclaimer.
30
// + this list of conditions and the following disclaimer.
Line 281... Line 281...
281
   * Set variables also.
281
   * Set variables also.
282
   */
282
   */
283
  controlMixer_update();
283
  controlMixer_update();
Line 284... Line 284...
284
 
284
 
285
  throttleTerm = controlThrottle;
285
  throttleTerm = controlThrottle;
Line 286... Line 286...
286
  if(throttleTerm < staticParams.GasMin + 10) throttleTerm = staticParams.GasMin + 10;
286
  if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
287
 
287
 
288
  /************************************************************************/
288
  /************************************************************************/
289
  /* RC-signal is bad                                                     */
289
  /* RC-signal is bad                                                     */
Line 457... Line 457...
457
   */
457
   */
458
  if(missingMotor) {
458
  if(missingMotor) {
459
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
459
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
460
    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
460
    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
461
      isFlying = 1; // keep within lift off condition
461
      isFlying = 1; // keep within lift off condition
462
    throttleTerm = staticParams.GasMin; // reduce gas to min to avoid lift of
462
    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
463
  }
463
  }
Line 464... Line 464...
464
 
464
 
465
  /*
465
  /*
466
   * Height control was here.
466
   * Height control was here.
467
   */
467
   */
468
  if(throttleTerm > staticParams.GasMax - 20) throttleTerm = (staticParams.GasMax - 20);
468
  if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
Line 469... Line 469...
469
  throttleTerm *= STICK_GAIN;
469
  throttleTerm *= STICK_GAIN;
470
 
470
 
471
  /*
471
  /*
Line 484... Line 484...
484
     * -20 <= yawTerm <= 20
484
     * -20 <= yawTerm <= 20
485
     */
485
     */
486
    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
486
    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
487
  }
487
  }
Line 488... Line 488...
488
 
488
 
Line 489... Line 489...
489
  tmp_int = staticParams.GasMax * STICK_GAIN;
489
  tmp_int = staticParams.MaxThrottle * STICK_GAIN;
490
 
490
 
491
  /*
491
  /*
492
   * throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle
492
   * throttle-MaxThrottle <= yawTerm <= MaxThrottle-throttle
Line 537... Line 537...
537
      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;
537
      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;
538
      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;
538
      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;
539
      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
539
      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
540
      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
540
      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
541
      tmp = motorFilters[i] / STICK_GAIN;
541
      tmp = motorFilters[i] / STICK_GAIN;
542
      CHECK_MIN_MAX(tmp, staticParams.GasMin, staticParams.GasMax);
542
      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
543
      Motor[i].SetPoint = tmp;
543
      Motor[i].SetPoint = tmp;
544
    }
544
    }
545
    else Motor[i].SetPoint = 0;
545
    else Motor[i].SetPoint = 0;
546
  }
546
  }