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 |