7,20 → 7,37 |
|
#include <inttypes.h> |
|
#define YAW_GYRO_DEG_FACTOR 1291L // Factor between Yaw Gyro Integral and HeadingAngle in deg |
#define STICK_GAIN 4 |
// 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 Height_D; |
uint8_t HeightD; |
uint8_t MaxHeight; |
uint8_t Height_P; |
uint8_t HeightP; |
uint8_t Height_ACC_Effect; |
uint8_t CompassYawEffect; |
uint8_t Gyro_P; |
uint8_t Gyro_I; |
uint8_t Yaw_P; |
uint8_t I_Factor; |
uint8_t GyroD; |
uint8_t GyroP; |
uint8_t GyroI; |
uint8_t StickYawP; |
uint8_t IFactor; |
uint8_t UserParam1; |
uint8_t UserParam2; |
uint8_t UserParam3; |
31,8 → 48,9 |
uint8_t UserParam8; |
uint8_t ServoNickControl; |
uint8_t LoopGasLimit; |
uint8_t Yaw_PosFeedback; |
uint8_t Yaw_NegFeedback; |
uint8_t AxisCoupling1; |
uint8_t AxisCoupling2; |
uint8_t AxisCouplingYawCorrection; |
uint8_t DynamicStability; |
uint8_t ExternalControl; |
uint8_t J16Timing; |
48,30 → 66,33 |
uint8_t NaviWindCorrection; |
uint8_t NaviSpeedCompensation; |
#endif |
int8_t Kalman_K; |
int8_t Kalman_MaxDrift; |
int8_t Kalman_MaxFusion; |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
} fc_param_t; |
|
extern fc_param_t FCParam; |
|
// attitude |
extern int32_t IntegralNick, IntegralRoll, IntegralYaw; |
extern int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw; |
|
// offsets |
extern int16_t AdNeutralNick, AdNeutralRoll, AdNeutralYaw; |
extern volatile int16_t NeutralAccX, NeutralAccY; |
extern volatile float NeutralAccZ; |
// rotation rates |
extern int16_t GyroNick, GyroRoll, GyroYaw; |
|
// attitude calcualted by temporal integral of gyro rates |
extern int32_t IntegralGyroNick, IntegralGyroRoll, IntegralGyroYaw; |
|
extern volatile int32_t Reading_Integral_Top; // calculated in analog.c |
|
// 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 volatile int16_t CompassHeading; |
extern volatile int16_t CompassCourse; |
extern volatile int16_t CompassOffCourse; |
extern volatile uint8_t CompassCalState; |
extern int16_t CompassHeading; |
extern int16_t CompassCourse; |
extern int16_t CompassOffCourse; |
extern uint8_t CompassCalState; |
extern int32_t YawGyroHeading; |
extern int16_t YawGyroHeadingInDeg; |
|
79,8 → 100,8 |
extern int ReadingHeight; |
extern int SetPointHeight; |
|
// mean accelerations |
extern int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop; |
// accelerations |
extern int16_t AccNick, AccRoll, AccTop; |
|
// acceleration send to navi board |
extern int16_t NaviAccNick, NaviAccRoll, NaviCntAcc; |
92,12 → 113,12 |
// external control |
extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
|
#define ACC_CALIB 1 |
#define NO_ACC_CALIB 0 |
|
void MotorControl(void); |
void SendMotorData(void); |
void CalibMean(void); |
void Mean(void); |
void SetNeutral(void); |
void SetNeutral(uint8_t AccAdjustment); |
void Beep(uint8_t numbeeps); |
|
|
104,20 → 125,21 |
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8; |
|
// setpoints for motors |
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr |
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; |
extern int16_t GPS_Nick; |
extern int16_t GPS_Roll; |
// current GPS-stick values |
extern int16_t GPSStickNick; |
extern int16_t GPSStickRoll; |
|
// current stick elongations |
extern int16_t MaxStickNick, MaxStickRoll, MaxStickYaw; |
|
|
extern uint16_t Model_Is_Flying; |
extern uint16_t ModelIsFlying; |
|
|
// MKFlags |
130,7 → 152,7 |
#define MKFLAG_RESERVE2 0x40 |
#define MKFLAG_RESERVE3 0x80 |
|
volatile extern uint8_t MKFlags; |
extern uint8_t MKFlags; |
|
#endif //_FC_H |
|