Subversion Repositories FlightCtrl

Rev

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

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