Subversion Repositories FlightCtrl

Rev

Rev 1961 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1961 Rev 1963
Line 2... Line 2...
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 und 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 Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
18
// + eindeutig als Ursprung verlinkt und genannt 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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 42... Line 42...
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
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
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48
// +  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
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49
// +  POSSIBILITY OF SUCH DAMAGE.
50
// +  POSSIBILITY OF SUCH DAMAGE.
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
 
-
 
52
/************************************************************************/
52
/************************************************************************/
53
/* Flight Attitude                                                      */
53
/* Flight Attitude                                                      */
54
/************************************************************************/
54
/************************************************************************/
Line 55... Line 55...
55
 
55
 
Line 204... Line 204...
204
  uint8_t axis;
204
  uint8_t axis;
Line 205... Line 205...
205
 
205
 
Line 206... Line 206...
206
  analog_update();
206
  analog_update();
207
 
207
 
208
  for (axis = PITCH; axis <= ROLL; axis++) {
208
  for (axis = PITCH; axis <= ROLL; axis++) {
209
    rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
209
    rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
210
    rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis];
210
    rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
211
    differential[axis] = gyroD[axis];
211
    differential[axis] = gyroD[axis];
Line 212... Line 212...
212
    averageAcc[axis] += acc[axis];
212
    averageAcc[axis] += acc[axis];
Line 246... Line 246...
246
// 480 usec with axis coupling - almost no time without.
246
// 480 usec with axis coupling - almost no time without.
247
void integrate(void) {
247
void integrate(void) {
248
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
248
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
249
  uint8_t axis;
249
  uint8_t axis;
Line 250... Line 250...
250
 
250
 
251
  if (/*!looping && */ (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) {
251
  if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) {
252
    trigAxisCoupling();
252
    trigAxisCoupling();
253
  } else {
253
  } else {
254
    ACRate[PITCH] = rate_ATT[PITCH];
254
    ACRate[PITCH] = rate_ATT[PITCH];
255
    ACRate[ROLL] = rate_ATT[ROLL];
255
    ACRate[ROLL] = rate_ATT[ROLL];
Line 312... Line 312...
312
    if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
312
    if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
313
      permilleAcc /= 2;
313
      permilleAcc /= 2;
314
      debugFullWeight = 0;
314
      debugFullWeight = 0;
315
    */
315
    */
Line 316... Line 316...
316
 
316
 
317
    if (controlActivity > 10000) { // reduce effect during stick commands
317
    if (controlActivity > 10000) { // reduce effect during stick control activity
318
      permilleAcc /= 4;
318
      permilleAcc /= 4;
319
      debugOut.digital[0] |= DEBUG_ACC0THORDER;
319
      debugOut.digital[0] |= DEBUG_ACC0THORDER;
320
      if (controlActivity > 20000) { // reduce effect during stick commands
320
      if (controlActivity > 20000) { // reduce effect during stick control activity
321
        permilleAcc /= 4;
321
        permilleAcc /= 4;
322
        debugOut.digital[1] |= DEBUG_ACC0THORDER;
322
        debugOut.digital[1] |= DEBUG_ACC0THORDER;
323
      }
323
      }
Line 329... Line 329...
329
    for (axis = PITCH; axis <= ROLL; axis++) {
329
    for (axis = PITCH; axis <= ROLL; axis++) {
330
      accDerived = getAngleEstimateFromAcc(axis);
330
      accDerived = getAngleEstimateFromAcc(axis);
331
      debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
331
      debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
Line 332... Line 332...
332
 
332
 
333
      // 1000 * the correction amount that will be added to the gyro angle in next line.
333
      // 1000 * the correction amount that will be added to the gyro angle in next line.
334
      temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
334
      temp = angle[axis];
335
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
335
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
336
          + (int32_t) permilleAcc * accDerived) / 1000L;
336
          + (int32_t) permilleAcc * accDerived) / 1000L;
337
      correctionSum[axis] += angle[axis] - temp;
337
      correctionSum[axis] += angle[axis] - temp;
338
    }
338
    }
339
  } else {
339
  } else {
340
    debugOut.analog[9] = 0;
340
    debugOut.analog[9] = 0;
Line 341... Line 341...
341
    debugOut.analog[10] = 0;
341
    debugOut.analog[10] = 0;
342
 
342
 
343
    // experiment: Kill drift compensation updates when not flying smooth.
343
    // experiment: Kill drift compensation updates when not flying smooth.
344
    correctionSum[PITCH] = correctionSum[ROLL] = 0;
344
    // correctionSum[PITCH] = correctionSum[ROLL] = 0;
Line 345... Line 345...
345
  }
345
  }
346
}
346
}