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)

// shift for zero centered rc channel data to Poty and thrust values
#define RC_POTI_OFFSET 110
#define RC_GAS_OFFSET 120

extern uint8_t RequiredMotors;

typedef struct
{
        uint8_t HeightD;
        uint8_t MaxHeight;
        uint8_t HeightP;
        uint8_t Height_ACC_Effect;
        uint8_t Height_GPS_Z;
        uint8_t CompassYawEffect;
        uint8_t GyroD;
        uint8_t GyroP;
        uint8_t GyroI;
        uint8_t GyroYawP;
        uint8_t GyroYawI;
        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 ServoRollControl;
        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;

// height control
extern int32_t 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, uint16_t duration);


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_LOWBAT                           0x20
#define MKFLAG_RESERVE2                         0x40
#define MKFLAG_RESERVE3                         0x80

extern volatile uint8_t MKFlags;

#endif //_FC_H