Rev 1539 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1538 | killagreg | 1 | /*####################################################################################### |
| 2 | Flight Control |
||
| 3 | #######################################################################################*/ |
||
| 4 | |||
| 5 | #ifndef _FC_H |
||
| 6 | #define _FC_H |
||
| 7 | |||
| 8 | #include <inttypes.h> |
||
| 9 | |||
| 10 | // scaling from AdAccNick, AdAccRoll -> AccNick, AccRoll |
||
| 11 | // i.e. AccNick = ACC_AMPLIFY * AdAccNick |
||
| 12 | #define ACC_AMPLIFY 6 |
||
| 13 | |||
| 14 | // scaling from AccNick, AccRoll -> Attitude in deg (approx sin(x) = x), |
||
| 15 | // i.e. Nick Angle in deg = AccNick / ACC_DEG_FACTOR |
||
| 16 | |||
| 17 | // the value is derived from the datasheet of the ACC sensor where 5g are scaled to vref |
||
| 18 | // therefore 1g is 1024/5 = 205 counts. the adc isr combines 2 acc samples to AdValueAcc |
||
| 19 | // and 1g yields to AdValueAcc = 2* 205 * 410 wich is again scaled by ACC_DEG_FACTOR |
||
| 20 | // that results in 1g --> Acc = 205 * 12 = 2460. the linear approx of the arcsin and the scaling |
||
| 21 | // of Acc gives the factor below. sin(20deg) * 2460 = 841 --> 841 / 20 = 42 |
||
| 22 | #define ACC_DEG_FACTOR 42 |
||
| 23 | |||
| 24 | // scaling from IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw -> Attitude in deg |
||
| 25 | // i.e. Nick Angle in deg = IntegralGyroNick / GYRO_DEG_FACTOR |
||
| 26 | #define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR) |
||
| 27 | |||
| 28 | // shift for zero centered rc channel data to Poty and thrust values |
||
| 29 | #define RC_POTI_OFFSET 110 |
||
| 30 | #define RC_GAS_OFFSET 120 |
||
| 31 | |||
| 32 | extern uint8_t RequiredMotors; |
||
| 33 | |||
| 34 | typedef struct |
||
| 35 | { |
||
| 36 | uint8_t HeightD; |
||
| 37 | uint8_t MaxHeight; |
||
| 38 | uint8_t HeightP; |
||
| 39 | uint8_t Height_ACC_Effect; |
||
| 40 | uint8_t Height_GPS_Z; |
||
| 41 | uint8_t CompassYawEffect; |
||
| 42 | uint8_t GyroD; |
||
| 43 | uint8_t GyroP; |
||
| 44 | uint8_t GyroI; |
||
| 45 | uint8_t GyroYawP; |
||
| 46 | uint8_t GyroYawI; |
||
| 47 | uint8_t StickYawP; |
||
| 48 | uint8_t IFactor; |
||
| 49 | uint8_t UserParam1; |
||
| 50 | uint8_t UserParam2; |
||
| 51 | uint8_t UserParam3; |
||
| 52 | uint8_t UserParam4; |
||
| 53 | uint8_t UserParam5; |
||
| 54 | uint8_t UserParam6; |
||
| 55 | uint8_t UserParam7; |
||
| 56 | uint8_t UserParam8; |
||
| 57 | uint8_t ServoNickControl; |
||
| 58 | uint8_t ServoRollControl; |
||
| 59 | uint8_t LoopGasLimit; |
||
| 60 | uint8_t AxisCoupling1; |
||
| 61 | uint8_t AxisCoupling2; |
||
| 62 | uint8_t AxisCouplingYawCorrection; |
||
| 63 | uint8_t DynamicStability; |
||
| 64 | uint8_t ExternalControl; |
||
| 65 | uint8_t J16Timing; |
||
| 66 | uint8_t J17Timing; |
||
| 67 | #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
||
| 68 | uint8_t NaviGpsModeControl; |
||
| 69 | uint8_t NaviGpsGain; |
||
| 70 | uint8_t NaviGpsP; |
||
| 71 | uint8_t NaviGpsI; |
||
| 72 | uint8_t NaviGpsD; |
||
| 73 | uint8_t NaviGpsACC; |
||
| 74 | uint8_t NaviOperatingRadius; |
||
| 75 | uint8_t NaviWindCorrection; |
||
| 76 | uint8_t NaviSpeedCompensation; |
||
| 77 | #endif |
||
| 78 | int8_t KalmanK; |
||
| 79 | int8_t KalmanMaxDrift; |
||
| 80 | int8_t KalmanMaxFusion; |
||
| 81 | } fc_param_t; |
||
| 82 | |||
| 83 | extern fc_param_t FCParam; |
||
| 84 | |||
| 85 | |||
| 86 | // rotation rates |
||
| 87 | extern int16_t GyroNick, GyroRoll, GyroYaw; |
||
| 88 | |||
| 89 | // attitude calcualted by temporal integral of gyro rates |
||
| 90 | extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw; |
||
| 91 | |||
| 92 | |||
| 93 | // bias values |
||
| 94 | extern int16_t BiasHiResGyroNick, BiasHiResGyroRoll, AdBiasGyroYaw; |
||
| 95 | extern int16_t AdBiasAccNick, AdBiasAccRoll; |
||
| 96 | extern volatile float AdBiasAccTop; |
||
| 97 | |||
| 98 | extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
||
| 99 | |||
| 100 | // compass navigation |
||
| 101 | extern int16_t CompassHeading; |
||
| 102 | extern int16_t CompassCourse; |
||
| 103 | extern int16_t CompassOffCourse; |
||
| 104 | extern uint8_t CompassCalState; |
||
| 105 | extern int32_t YawGyroHeading; |
||
| 106 | extern int16_t YawGyroHeadingInDeg; |
||
| 107 | |||
| 108 | // height control |
||
| 109 | extern int32_t SetPointHeight; |
||
| 110 | |||
| 111 | // accelerations |
||
| 112 | extern int16_t AccNick, AccRoll, AccTop; |
||
| 113 | |||
| 114 | // acceleration send to navi board |
||
| 115 | extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
||
| 116 | |||
| 117 | |||
| 118 | // looping params |
||
| 119 | extern long TurnOver180Nick, TurnOver180Roll; |
||
| 120 | |||
| 121 | // external control |
||
| 122 | extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
||
| 123 | |||
| 124 | #define ACC_CALIB 1 |
||
| 125 | #define NO_ACC_CALIB 0 |
||
| 126 | |||
| 127 | void MotorControl(void); |
||
| 128 | void SendMotorData(void); |
||
| 129 | void SetNeutral(uint8_t AccAdjustment); |
||
| 130 | void Beep(uint8_t numbeeps, uint16_t duration); |
||
| 131 | |||
| 132 | |||
| 133 | extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
||
| 134 | |||
| 135 | // current stick values |
||
| 136 | extern int16_t StickNick; |
||
| 137 | extern int16_t StickRoll; |
||
| 138 | extern int16_t StickYaw; |
||
| 139 | // current GPS-stick values |
||
| 140 | extern int16_t GPSStickNick; |
||
| 141 | extern int16_t GPSStickRoll; |
||
| 142 | |||
| 143 | // current stick elongations |
||
| 144 | extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
||
| 145 | |||
| 146 | |||
| 147 | extern uint16_t ModelIsFlying; |
||
| 148 | |||
| 149 | |||
| 150 | // MKFlags |
||
| 151 | #define MKFLAG_MOTOR_RUN 0x01 |
||
| 152 | #define MKFLAG_FLY 0x02 |
||
| 153 | #define MKFLAG_CALIBRATE 0x04 |
||
| 154 | #define MKFLAG_START 0x08 |
||
| 155 | #define MKFLAG_EMERGENCY_LANDING 0x10 |
||
| 156 | #define MKFLAG_LOWBAT 0x20 |
||
| 157 | #define MKFLAG_RESERVE2 0x40 |
||
| 158 | #define MKFLAG_RESERVE3 0x80 |
||
| 159 | |||
| 160 | extern volatile uint8_t MKFlags; |
||
| 161 | |||
| 162 | #endif //_FC_H |
||
| 163 |