| 1,19 → 1,66 |
|---|
| #include <inttypes.h> |
| #include "controlMixer.h" |
| #include "attitude.h" |
| #include "compassControl.h" |
| #include <stdlib.h> |
| |
| // The only possibility in a compass control is to aim the head at some compass heading. |
| // One way could be: When a switch is turned on, we capture the current heading in a variable. |
| // From then on, if the current heading is to the right of the captured heading, we yaw left etc. |
| // If the yaw at input is nonzero, we could also temporarily disable this and instead re-capture |
| // the current heading.. |
| // 4 modes of control (like with the simple height controller): |
| // 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled |
| // 1) Heading is captured at takeoff, and held there (plus / minus bending via remote) |
| // 2) A variable controls the heading (plus / minus bending via remote) |
| // 3) Navigation controls the heading (plus / minus bending via remote) |
| |
| // The bending should be linear, or rotation rate controlled. |
| // The target heading variable is stored here (not in the flight or attitude modules), as here |
| // is where the regulation takes place. Also, it was found that a target heading was difficult |
| // to maintain in the flight module and the flight module would sometimes need to write back |
| // to it. The only possible (but sufficient) output from here is PRTY[CONTROL_YAW]. |
| |
| int32_t navigationTargetHeading; |
| int32_t magneticTargetHeading; |
| int32_t bending = 0; |
| |
| void compass_setTakeoffHeading(int32_t heading) { |
| magneticTargetHeading = heading; |
| } |
| |
| void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
| int16_t currentYaw = PRTY[CONTROL_YAW]; |
| if (abs(currentYaw) < 10) { |
| magneticTargetHeading = magneticHeading; |
| |
| switch (staticParams.compassMode) { |
| case COMPASS_MODE_OFF: |
| bending = 0; |
| break; |
| case COMPASS_MODE_TAKEOFF: |
| // just leave the target heading untouched! It should have been set at takeoff. |
| break; |
| // case COMPASS_MODE_RCVARIABLE: |
| // magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8); |
| // break; |
| case COMPASS_MODE_NAVIGATION: |
| magneticTargetHeading = navigationTargetHeading; |
| break; |
| } |
| |
| if (staticParams.compassMode != COMPASS_MODE_OFF) { |
| if (abs(currentYaw) >= staticParams.naviStickThreshold) { |
| // Bending: We do not actually need to do anything here, as the stick movement behing the bending will |
| // pass right thru and do its stuff in the flight module. |
| // All we have to do is to not counteract bending, and to return somewhat gracefully to target heading |
| // again (using what parameter as speed?) when pilot stops bending. |
| bending += currentYaw; |
| } else { |
| if (bending > staticParams.compassBendingReturnSpeed) { |
| bending -= staticParams.compassBendingReturnSpeed; |
| } else if (bending < -staticParams.compassBendingReturnSpeed) { |
| bending += staticParams.compassBendingReturnSpeed; |
| } else |
| bending = 0; |
| } |
| |
| // We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID). |
| // Bending blends in like: (magneticTargetHeading +- bending - heading) |
| } |
| |
| |
| } |