Subversion Repositories FlightCtrl

Rev

Rev 1612 | 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 491... Line 491...
491
   */
491
   */
492
  if(missingMotor) {
492
  if(missingMotor) {
493
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
493
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
494
    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
494
    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
495
      isFlying = 1; // keep within lift off condition
495
      isFlying = 1; // keep within lift off condition
496
    throttleTerm = staticParams.GasMin; // reduce gas to min to avoid lift of
496
    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
497
  }
497
  }
Line 498... Line 498...
498
 
498
 
499
  /*
499
  /*
500
   * Height control was here.
500
   * Height control was here.
Line 501... Line 501...
501
   */
501
   */
502
 
502
 
Line 503... Line 503...
503
  if(throttleTerm > staticParams.GasMax - 20) throttleTerm = (staticParams.GasMax - 20);
503
  if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
504
  throttleTerm *= STICK_GAIN;
504
  throttleTerm *= STICK_GAIN;
505
 
505
 
Line 513... Line 513...
513
    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
513
    CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
514
  } else {
514
  } else {
515
    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
515
    CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
516
  }
516
  }
Line 517... Line 517...
517
 
517
 
518
  tmp_int = staticParams.GasMax * STICK_GAIN;
518
  tmp_int = staticParams.MaxThrottle * STICK_GAIN;
Line 519... Line 519...
519
  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
519
  CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
520
 
520
 
521
  /*
521
  /*
Line 562... Line 562...
562
      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;
562
      tmp += ((int32_t)pitchTerm    * Mixer.Motor[i][MIX_PITCH])    / 64L;
563
      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;
563
      tmp += ((int32_t)rollTerm     * Mixer.Motor[i][MIX_ROLL])     / 64L;
564
      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
564
      tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
565
      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
565
      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
566
      tmp = motorFilters[i] / STICK_GAIN;
566
      tmp = motorFilters[i] / STICK_GAIN;
567
      CHECK_MIN_MAX(tmp, staticParams.GasMin, staticParams.GasMax);
567
      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
568
      Motor[i].SetPoint = tmp;
568
      Motor[i].SetPoint = tmp;
569
    }
569
    }
570
    else Motor[i].SetPoint = 0;
570
    else Motor[i].SetPoint = 0;
571
  }
571
  }