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