Rev 2052 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2052 | Rev 2189 | ||
|---|---|---|---|
| 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> |
| 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 |
| 9 | // 1) Heading is captured at takeoff, and held there (plus / minus bending via remote) |
10 | // 1) Heading is captured at takeoff, and held there (plus / minus bending via remote) |
| 10 | // 2) A variable controls the heading (plus / minus bending via remote) |
11 | // 2) A variable controls the heading (plus / minus bending via remote) |
| 11 | // 3) Navigation controls the heading (plus / minus bending via remote) |
12 | // 3) Navigation controls the heading (plus / minus bending via remote) |
| 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 |
| 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; |
| 20 | int32_t magneticTargetHeading; |
21 | int32_t magneticTargetHeading; |
| 21 | int32_t bending = 0; |
22 | int32_t bending; |
| 22 | 23 | ||
| 23 | void compass_setTakeoffHeading(int32_t heading) { |
24 | void compass_setTakeoffHeading(int32_t heading) { |
| 24 | magneticTargetHeading = heading; |
25 | magneticTargetHeading = heading; |
| 25 | } |
26 | } |
| 26 | 27 | ||
| 27 | void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
28 | void CC_periodicTaskAndRPTY(int16_t* RPTY) { |
| 28 | int16_t currentYaw = PRTY[CONTROL_YAW]; |
29 | int16_t currentYaw = RPTY[CONTROL_YAW]; |
| 29 | 30 | ||
| 30 | switch (staticParams.compassMode) { |
31 | switch (staticParams.compassMode) { |
| 31 | case COMPASS_MODE_OFF: |
32 | case COMPASS_MODE_OFF: |
| - | 33 | default: |
|
| 32 | bending = 0; |
34 | bending = 0; |
| 33 | break; |
35 | break; |
| 34 | case COMPASS_MODE_TAKEOFF: |
36 | case COMPASS_MODE_TAKEOFF: |
| 35 | // just leave the target heading untouched! It should have been set at takeoff. |
37 | // just leave the target heading untouched! It should have been set at takeoff. |
| 36 | break; |
38 | break; |
| 37 | // case COMPASS_MODE_RCVARIABLE: |
39 | // case COMPASS_MODE_RCVARIABLE: |
| 38 | // magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8); |
40 | // magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8); |
| 39 | // break; |
41 | // break; |
| 40 | case COMPASS_MODE_NAVIGATION: |
42 | case COMPASS_MODE_NAVIGATION: |
| 41 | magneticTargetHeading = navigationTargetHeading; |
43 | magneticTargetHeading = navigationTargetHeading; |
| 42 | break; |
44 | break; |
| 43 | } |
45 | } |
| 44 | 46 | ||
| 45 | if (staticParams.compassMode != COMPASS_MODE_OFF) { |
47 | if (staticParams.compassMode != COMPASS_MODE_OFF) { |
| 46 | if (abs(currentYaw) >= staticParams.naviStickThreshold) { |
48 | if (abs(currentYaw) >= staticParams.naviStickThreshold) { |
| 47 | // Bending: We do not actually need to do anything here, as the stick movement behing the bending will |
49 | // 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. |
50 | // 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 |
51 | // 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. |
52 | // again (using what parameter as speed?) when pilot stops bending. |
| 51 | bending += currentYaw; |
53 | bending += currentYaw; |
| 52 | } else { |
54 | } else { |
| 53 | if (bending > staticParams.compassBendingReturnSpeed) { |
55 | if (bending > staticParams.compassBendingReturnSpeed) { |
| 54 | bending -= staticParams.compassBendingReturnSpeed; |
56 | bending -= staticParams.compassBendingReturnSpeed; |
| 55 | } else if (bending < -staticParams.compassBendingReturnSpeed) { |
57 | } else if (bending < -staticParams.compassBendingReturnSpeed) { |
| 56 | bending += staticParams.compassBendingReturnSpeed; |
58 | bending += staticParams.compassBendingReturnSpeed; |
| 57 | } else |
59 | } else |
| 58 | bending = 0; |
60 | bending = 0; |
| 59 | } |
61 | } |
| 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 | } |
65 | } |
| 64 | - | ||
| 65 | - | ||
| 66 | } |
66 | } |
| 67 | 67 | ||