Rev 2048 | 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" |
| 2048 | - | 5 | #include <stdlib.h> |
| 6 | |||
| 2052 | - | 7 | // 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 | // 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 | // 3) Navigation controls the heading (plus / minus bending via remote) |
||
| 2048 | - | 12 | |
| 2052 | - | 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]. |
||
| 18 | |||
| 19 | int32_t navigationTargetHeading; |
||
| 2048 | - | 20 | int32_t magneticTargetHeading; |
| 2052 | - | 21 | int32_t bending = 0; |
| 2048 | - | 22 | |
| 2052 | - | 23 | void compass_setTakeoffHeading(int32_t heading) { |
| 24 | magneticTargetHeading = heading; |
||
| 25 | } |
||
| 26 | |||
| 2048 | - | 27 | void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
| 28 | int16_t currentYaw = PRTY[CONTROL_YAW]; |
||
| 2052 | - | 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: |
||
| 38 | // magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8); |
||
| 39 | // break; |
||
| 40 | case COMPASS_MODE_NAVIGATION: |
||
| 41 | magneticTargetHeading = navigationTargetHeading; |
||
| 42 | break; |
||
| 2048 | - | 43 | } |
| 2052 | - | 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) |
||
| 63 | } |
||
| 64 | |||
| 65 | |||
| 2048 | - | 66 | } |