Subversion Repositories FlightCtrl

/*#######################################################################################

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)

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;

// setpoints for motors

extern volatile uint8_t Motor1, Motor2, Motor3, Motor4, Motor5, Motor6, Motor7, Motor8;

// 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 uint8_t MKFlags;

#endif //_FC_H