Subversion Repositories FlightCtrl

Rev

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

Rev 2052 Rev 2189
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 "compassControl.h"
- 
 
 5 
#include "definitions.h"
5 
#include <stdlib.h>
 6 
#include <stdlib.h>
Line 6... Line 7...
6 
 
 7 
 
7 
// 4 modes of control (like with the simple height controller):
 8 
// 4 modes of control (like with the simple height controller):
8 
// 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled
 9 
// 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled
Line 12... Line 13...
12 
 
 13 
 
13 
// The bending should be linear, or rotation rate controlled.
 14 
// 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 
// 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 
// 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 maintain in the flight module and the flight module would sometimes need to write back
Line 17... Line 18...
17 
// to it. The only possible (but sufficient) output from here is PRTY[CONTROL_YAW].
 18 
// to it. The only possible (but sufficient) output from here is RPTY[CONTROL_YAW].
18 
 
 19 
 
19 
int32_t navigationTargetHeading;
 20 
int32_t navigationTargetHeading;
Line 20... Line 21...
20 
int32_t magneticTargetHeading;
 21 
int32_t magneticTargetHeading;
21 
int32_t bending = 0;
 22 
int32_t bending;
22 
 
 23 
 
Line 23... Line 24...
23 
void compass_setTakeoffHeading(int32_t heading) {
 24 
void compass_setTakeoffHeading(int32_t heading) {
24 
  magneticTargetHeading = heading;
 25 
  magneticTargetHeading = heading;
Line 25... Line 26...
25 
}
 26 
}
26 
 
 27 
 
- 
 
 28 
void CC_periodicTaskAndRPTY(int16_t* RPTY) {
27 
void CC_periodicTaskAndPRTY(int16_t* PRTY) {
 29 
  int16_t currentYaw = RPTY[CONTROL_YAW];
28 
  int16_t currentYaw = PRTY[CONTROL_YAW];
 30 
 
29 
 
 31 
  switch (staticParams.compassMode) {
30 
  switch (staticParams.compassMode) {
 32 
  case COMPASS_MODE_OFF:
31 
  case COMPASS_MODE_OFF:
 33 
  default:
Line 59... Line 61...
59 
    }
 61 
    }
Line 60... Line 62...
60 
 
 62 
 
61 
    // We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID).
 63 
    // 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)
 64 
    // Bending blends in like: (magneticTargetHeading +- bending - heading)
63 
  }
 - 
 
64 
 
 - 
 
65 
 
 65 
  }