| 2,6 → 2,7 |
|---|
| #include "controlMixer.h" |
| #include "attitude.h" |
| #include "compassControl.h" |
| #include "definitions.h" |
| #include <stdlib.h> |
| |
| // 4 modes of control (like with the simple height controller): |
| 14,21 → 15,22 |
|---|
| // 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]. |
| // to it. The only possible (but sufficient) output from here is RPTY[CONTROL_YAW]. |
| |
| int32_t navigationTargetHeading; |
| int32_t magneticTargetHeading; |
| int32_t bending = 0; |
| int32_t bending; |
| |
| void compass_setTakeoffHeading(int32_t heading) { |
| magneticTargetHeading = heading; |
| } |
| |
| void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
| int16_t currentYaw = PRTY[CONTROL_YAW]; |
| void CC_periodicTaskAndRPTY(int16_t* RPTY) { |
| int16_t currentYaw = RPTY[CONTROL_YAW]; |
| |
| switch (staticParams.compassMode) { |
| case COMPASS_MODE_OFF: |
| default: |
| bending = 0; |
| break; |
| case COMPASS_MODE_TAKEOFF: |
| 61,6 → 63,4 |
|---|
| // We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID). |
| // Bending blends in like: (magneticTargetHeading +- bending - heading) |
| } |
| |
| |
| } |