Subversion Repositories FlightCtrl

Rev

Rev 2052 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2048 - 1
#include <inttypes.h>
2
#include "controlMixer.h"
3
#include "attitude.h"
2052 - 4
#include "compassControl.h"
2189 - 5
#include "definitions.h"
2048 - 6
#include <stdlib.h>
7
 
2052 - 8
// 4 modes of control (like with the simple height controller):
9
// 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled
10
// 1) Heading is captured at takeoff, and held there (plus / minus bending via remote)
11
// 2) A variable controls the heading (plus / minus bending via remote)
12
// 3) Navigation controls the heading (plus / minus bending via remote)
2048 - 13
 
2052 - 14
// The bending should be linear, or rotation rate controlled.
15
// The target heading variable is stored here (not in the flight or attitude modules), as here
16
// is where the regulation takes place. Also, it was found that a target heading was difficult
17
// to maintain in the flight module and the flight module would sometimes need to write back
2189 - 18
// to it. The only possible (but sufficient) output from here is RPTY[CONTROL_YAW].
2052 - 19
 
20
int32_t navigationTargetHeading;
2048 - 21
int32_t magneticTargetHeading;
2189 - 22
int32_t bending;
2048 - 23
 
2052 - 24
void compass_setTakeoffHeading(int32_t heading) {
25
  magneticTargetHeading = heading;
26
}
27
 
2189 - 28
void CC_periodicTaskAndRPTY(int16_t* RPTY) {
29
  int16_t currentYaw = RPTY[CONTROL_YAW];
2052 - 30
 
31
  switch (staticParams.compassMode) {
32
  case COMPASS_MODE_OFF:
2189 - 33
  default:
2052 - 34
    bending = 0;
35
    break;
36
  case COMPASS_MODE_TAKEOFF:
37
    // just leave the target heading untouched! It should have been set at takeoff.
38
    break;
39
//  case COMPASS_MODE_RCVARIABLE:
40
//    magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8);
41
//    break;
42
  case COMPASS_MODE_NAVIGATION:
43
    magneticTargetHeading = navigationTargetHeading;
44
    break;
2048 - 45
  }
2052 - 46
 
47
  if (staticParams.compassMode != COMPASS_MODE_OFF) {
48
    if (abs(currentYaw) >= staticParams.naviStickThreshold) {
49
      // Bending: We do not actually need to do anything here, as the stick movement behing the bending will
50
      // pass right thru and do its stuff in the flight module.
51
      // All we have to do is to not counteract bending, and to return somewhat gracefully to target heading
52
      // again (using what parameter as speed?) when pilot stops bending.
53
      bending += currentYaw;
54
    } else {
55
      if (bending > staticParams.compassBendingReturnSpeed) {
56
        bending -= staticParams.compassBendingReturnSpeed;
57
      } else if (bending < -staticParams.compassBendingReturnSpeed) {
58
        bending += staticParams.compassBendingReturnSpeed;
59
      } else
60
        bending = 0;
61
    }
62
 
63
    // We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID).
64
    // Bending blends in like: (magneticTargetHeading +- bending - heading)
65
  }
2048 - 66
}