| 0,0 → 1,157 |
|---|
| /*####################################################################################### |
| Flight Control |
| #######################################################################################*/ |
| |
| #ifndef _FC_H |
| #define _FC_H |
| |
| #include <inttypes.h> |
| |
| // scaling from AdAccNick, AdAccRoll -> AccNick, AccRoll |
| // i.e. AccNick = ACC_AMPLIFY * AdAccNick |
| #define ACC_AMPLIFY 6 |
| |
| // scaling from AccNick, AccRoll -> Attitude in deg (approx sin(x) = x), |
| // i.e. Nick Angle in deg = AccNick / ACC_DEG_FACTOR |
| |
| // the value is derived from the datasheet of the ACC sensor where 5g are scaled to vref |
| // therefore 1g is 1024/5 = 205 counts. the adc isr combines 2 acc samples to AdValueAcc |
| // and 1g yields to AdValueAcc = 2* 205 * 410 wich is again scaled by ACC_DEG_FACTOR |
| // that results in 1g --> Acc = 205 * 12 = 2460. the linear approx of the arcsin and the scaling |
| // of Acc gives the factor below. sin(20deg) * 2460 = 841 --> 841 / 20 = 42 |
| #define ACC_DEG_FACTOR 42 |
| |
| // scaling from IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw -> Attitude in deg |
| // i.e. Nick Angle in deg = IntegralGyroNick / GYRO_DEG_FACTOR |
| #define GYRO_DEG_FACTOR ((int16_t)(ParamSet.GyroAccFactor) * ACC_DEG_FACTOR) |
| |
| |
| extern uint8_t RequiredMotors; |
| |
| typedef struct |
| { |
| uint8_t HeightD; |
| uint8_t MaxHeight; |
| uint8_t HeightP; |
| uint8_t Height_ACC_Effect; |
| uint8_t CompassYawEffect; |
| uint8_t GyroD; |
| uint8_t GyroP; |
| uint8_t GyroI; |
| uint8_t StickYawP; |
| uint8_t IFactor; |
| uint8_t UserParam1; |
| uint8_t UserParam2; |
| uint8_t UserParam3; |
| uint8_t UserParam4; |
| uint8_t UserParam5; |
| uint8_t UserParam6; |
| uint8_t UserParam7; |
| uint8_t UserParam8; |
| uint8_t ServoNickControl; |
| uint8_t LoopGasLimit; |
| uint8_t AxisCoupling1; |
| uint8_t AxisCoupling2; |
| uint8_t AxisCouplingYawCorrection; |
| uint8_t DynamicStability; |
| uint8_t ExternalControl; |
| uint8_t J16Timing; |
| uint8_t J17Timing; |
| #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
| uint8_t NaviGpsModeControl; |
| uint8_t NaviGpsGain; |
| uint8_t NaviGpsP; |
| uint8_t NaviGpsI; |
| uint8_t NaviGpsD; |
| uint8_t NaviGpsACC; |
| uint8_t NaviOperatingRadius; |
| uint8_t NaviWindCorrection; |
| uint8_t NaviSpeedCompensation; |
| #endif |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| } fc_param_t; |
| |
| extern fc_param_t FCParam; |
| |
| |
| // rotation rates |
| extern int16_t GyroNick, GyroRoll, GyroYaw; |
| |
| // attitude calcualted by temporal integral of gyro rates |
| extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw; |
| |
| |
| // bias values |
| extern int16_t BiasHiResGyroNick, BiasHiResGyroRoll, AdBiasGyroYaw; |
| extern int16_t AdBiasAccNick, AdBiasAccRoll; |
| extern volatile float AdBiasAccTop; |
| |
| extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
| |
| // compass navigation |
| extern int16_t CompassHeading; |
| extern int16_t CompassCourse; |
| extern int16_t CompassOffCourse; |
| extern uint8_t CompassCalState; |
| extern int32_t YawGyroHeading; |
| extern int16_t YawGyroHeadingInDeg; |
| |
| // hight control |
| extern int ReadingHeight; |
| extern int SetPointHeight; |
| |
| // accelerations |
| extern int16_t AccNick, AccRoll, AccTop; |
| |
| // acceleration send to navi board |
| extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
| |
| |
| // looping params |
| extern long TurnOver180Nick, TurnOver180Roll; |
| |
| // external control |
| extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
| |
| #define ACC_CALIB 1 |
| #define NO_ACC_CALIB 0 |
| |
| void MotorControl(void); |
| void SendMotorData(void); |
| void SetNeutral(uint8_t AccAdjustment); |
| void Beep(uint8_t numbeeps); |
| |
| |
| extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
| |
| // current stick values |
| extern int16_t StickNick; |
| extern int16_t StickRoll; |
| extern int16_t StickYaw; |
| // current GPS-stick values |
| extern int16_t GPSStickNick; |
| extern int16_t GPSStickRoll; |
| |
| // current stick elongations |
| extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
| |
| |
| extern uint16_t ModelIsFlying; |
| |
| |
| // MKFlags |
| #define MKFLAG_MOTOR_RUN 0x01 |
| #define MKFLAG_FLY 0x02 |
| #define MKFLAG_CALIBRATE 0x04 |
| #define MKFLAG_START 0x08 |
| #define MKFLAG_EMERGENCY_LANDING 0x10 |
| #define MKFLAG_RESERVE1 0x20 |
| #define MKFLAG_RESERVE2 0x40 |
| #define MKFLAG_RESERVE3 0x80 |
| |
| extern volatile uint8_t MKFlags; |
| |
| #endif //_FC_H |
| |