Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 761 → Rev 762

/branches/V0.68d Code Redesign killagreg/GPS.c
1,5 → 1,4
#include <inttypes.h>
 
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
10,6 → 9,8
#define TSK_HOLD 1
#define TSK_HOME 2
 
#define GPS_STICK_SENSE 12
#define GPS_STICK_LIMIT 45
 
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
43,6 → 44,11
if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
}
 
// clear home position
void GPS_ClearHomePosition(void)
{
HomePosition.Status = INVALID;
}
 
// disable GPS control sticks
void GPS_Neutral(void)
51,68 → 57,82
GPS_Roll = 0;
}
 
// calculates the GPS control sticks values from the position deviation
void GPS_PDController(void)
// calculates the GPS control sticks values from the deviation to target position
void GPS_PDController(GPS_Pos_t *TargetPos)
{
int32_t coscompass, sincompass;
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
int32_t PD_North = 0, PD_East = 0;
//Calculate PD-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
{
GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting;
 
// GPS to pitch and roll settings
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
 
//A positive pitch angle moves head downwards (flying forward).
//A positive roll angle tilts left side downwards (flying left).
//Calculate PD-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
// If compass heading is 0 the head of the copter is in north direction.
// A positive pitch angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative pitch).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
 
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
// GPS to pitch and roll settings
 
// rotation transformation to compass heading to match any copter orientation
if (CompassHeading < 0) // no valid compass data
{ // disable GPS
GPS_Neutral();
//A positive pitch angle moves head downwards (flying forward).
//A positive roll angle tilts left side downwards (flying left).
 
// If compass heading is 0 the head of the copter is in north direction.
// A positive pitch angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative pitch).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
 
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
 
// rotation transformation to compass heading to match any copter orientation
if (CompassHeading < 0) // no valid compass data
{ // disable GPS
GPS_Neutral();
}
else
{
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
}
// limit GPS controls
if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;
if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT;
if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;
}
else
else // invalid input data
{
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
BeepTime = 50;
GPS_Neutral();
}
// limit GPS controls
#define GPS_CTRL_LIMIT 35
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;
}
 
 
void GPS_Main(void)
void GPS_Main(uint8_t ctrl)
{
static uint8_t GPS_Task = TSK_IDLE;
int16_t satbeep;
 
// poti2 enables the gps feature
if(Poti2 < 70) GPS_Task = TSK_IDLE;
else if (Poti2 < 160) GPS_Task = TSK_HOLD;
else GPS_Task = TSK_HOME; // Poti2 >= 160
// ctrl enables the gps feature
if(ctrl < 70) GPS_Task = TSK_IDLE;
else if (ctrl < 160) GPS_Task = TSK_HOLD;
else GPS_Task = TSK_HOME; // ctrl >= 160
 
 
switch(GPSInfo.status)
150,47 → 170,47
GPS_Neutral();
break; // eof TSK_IDLE
case TSK_HOLD:
// if sticks are centered and hold position is valid enable position hold control
if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
if(HoldPosition.Status == VALID)
{
// Calculate deviation from hold position
GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting;
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
 
GPS_PDController();
// if sticks are centered (no manual control)
if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
GPS_PDController(&HoldPosition);
}
else // MK controlled by user
{
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = GPSInfo.status;
// disable gps control
GPS_Neutral();
}
}
else // MK controlled by user
{
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
// disable gps control
GPS_Neutral();
}
break; // eof TSK_HOLD
case TSK_HOME:
if(HomePosition.Status == VALID)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
 
// Calculate deviation from home position
GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
 
GPS_PDController();
HoldPosition.Status = GPSInfo.status;
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
GPS_PDController(&HomePosition);
}
}
else // bad home position
{
GPS_Neutral();
BeepTime = 50; // signal invalid home position
// try to hold at least the position as a fallback option
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
{
GPS_PDController(&HoldPosition);
}
}
break; // eof TSK_HOME
default: // unhandled task
/branches/V0.68d Code Redesign killagreg/cmps03.c
10,9 → 10,9
/*********************************************/
void CMPS03_Init(void)
{
// Port PC4 connected to PWM output from compass module
// Port PC5 connected to PWM output from compass module
DDRC &= ~(1<<DDC4); // set as input
PORTC |= (1<<PORTC4); // pull up to increase PWM counter aslo if nowthing is connected
PORTC |= (1<<PORTC4); // pull up to increase PWM counter allo if nothing is connected
 
PWMTimeout = 0;
}
/branches/V0.68d Code Redesign killagreg/eeprom.c
83,7 → 83,7
ParamSet.GyroAccTrim = 16; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 100;
memcpy(ParamSet.Name, "Sport\0", 12);
memcpy(ParamSet.Name, "Sport\0",6);
}
 
 
146,7 → 146,7
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 75;
memcpy(ParamSet.Name, "Normal\0", 12);
memcpy(ParamSet.Name, "Normal\0", 7);
}
 
 
209,7 → 209,7
ParamSet.GyroAccTrim = 32; // 1/k
ParamSet.DriftComp = 4;
ParamSet.DynamicStability = 50;
memcpy(ParamSet.Name, "Beginner\0", 12);
memcpy(ParamSet.Name, "Beginner\0", 9);
}
 
/***************************************************/
/branches/V0.68d Code Redesign killagreg/fc.c
70,6 → 70,7
#ifdef USE_CMPS03
#include "cmps03.h"
#endif
#include "led.h"
 
volatile uint16_t I2CTimeout = 100;
// gyro readings
114,7 → 115,7
 
volatile int16_t DiffPitch, DiffRoll;
 
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 
// setpoints for motors
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
362,11 → 363,23
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
 
//PPM24-Extension
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
 
//limit poti values
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Extension
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
407,31 → 420,33
/************************************************************************/
void ParameterMapping(void)
{
 
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
 
Ki = (float) FCParam.I_Factor * FACTOR_I;
if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok
// else the last updated values are used
{
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
Ki = (float) FCParam.I_Factor * FACTOR_I;
}
}
 
 
440,24 → 455,24
/************************************************************************/
void MotorControl(void)
{
int16_t MotorValue, pd_result, h, tmp_int;
int16_t YawMixFraction, ThrustMixFraction;
static int32_t SumPitch = 0, SumRoll = 0;
static int32_t SetPointYaw = 0;
static int32_t IntegralErrorPitch = 0;
static int32_t IntegralErrorRoll = 0;
static uint16_t RcLostTimer;
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
static uint16_t Modell_Is_Flying = 0;
static uint8_t HeightControlActive = 0;
static int16_t HeightControlThrust = 0;
static int8_t TimerDebugOut = 0;
static int8_t StoreNewCompassCourse = 0;
static int32_t CorrectionPitch, CorrectionRoll;
int16_t MotorValue, pd_result, h, tmp_int;
int16_t YawMixFraction, ThrustMixFraction;
static int32_t SumPitch = 0, SumRoll = 0;
static int32_t SetPointYaw = 0;
static int32_t IntegralErrorPitch = 0;
static int32_t IntegralErrorRoll = 0;
static uint16_t RcLostTimer;
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
static uint16_t Modell_Is_Flying = 0;
static uint8_t HeightControlActive = 0;
static int16_t HeightControlThrust = 0;
static int8_t TimerDebugOut = 0;
static int8_t StoreNewCompassCourse = 0;
static int32_t CorrectionPitch, CorrectionRoll;
 
Mean();
GRN_ON;
 
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// determine thrust value
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
471,7 → 486,7
// Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80
// The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame
// the main loop is running 11 times that decrements the SenderOkay by 11.
if(SenderOkay < 100) // the rc-frame signal is not reveived or noisy
if(SenderOkay < 100) // the rc-frame signal is not reveived or noisy
{
if(!PcAccess) // if also no PC-Access via UART
{
500,7 → 515,7
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
}
else MotorsOn = 0; // switch of all motors
}
} // eof SenderOkay < 100
else
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// RC-signal is good
530,45 → 545,45
// calibrate the neutral readings of all attitude sensors
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_neutral = 0;
GRN_OFF;
Modell_Is_Flying = 0;
// check roll/pitch stick position
// if pitch stick is topmost or roll stick is leftmost --> change parameter setting
// according to roll/pitch stick position
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
uint8_t setting = 1; // default
// _________
// |2 3 4|
// | |
// |1 5|
// | |
// |_________|
//
// roll stick leftmost and pitch stick centered --> setting 1
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1;
// roll stick leftmost and pitch stick topmost --> setting 2
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2;
// roll stick centered an pitch stick topmost --> setting 3
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3;
// roll stick rightmost and pitch stick topmost --> setting 4
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4;
// roll stick rightmost and pitch stick centered --> setting 5
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5;
// update active parameter set in eeprom
SetActiveParamSet(setting);
delay_neutral = 0;
GRN_OFF;
Modell_Is_Flying = 0;
// check roll/pitch stick position
// if pitch stick is topmost or roll stick is leftmost --> change parameter setting
// according to roll/pitch stick position
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70)
{
uint8_t setting = 1; // default
// _________
// |2 3 4|
// | |
// |1 5|
// | |
// |_________|
//
// roll stick leftmost and pitch stick centered --> setting 1
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1;
// roll stick leftmost and pitch stick topmost --> setting 2
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2;
// roll stick centered an pitch stick topmost --> setting 3
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3;
// roll stick rightmost and pitch stick topmost --> setting 4
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4;
// roll stick rightmost and pitch stick centered --> setting 5
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5;
// update active parameter set in eeprom
SetActiveParamSet(setting);
}
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral();
Beep(GetActiveParamSet());
}
ParamSet_ReadFromEEProm(GetActiveParamSet());
SetNeutral();
Beep(GetActiveParamSet());
}
}
// and if the yaw stick is in the rightmost position
// save the ACC neutral setting to eeprom
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
{
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
584,7 → 599,7
Beep(GetActiveParamSet());
}
}
else delay_neutral = 0;
else delay_neutral = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// thrust stick is down
624,573 → 639,582
delay_stopmotors = 200; // do not repeat if once executed
Modell_Is_Flying = 0;
MotorsOn = 0;
 
GPS_ClearHomePosition();
}
}
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
}
}
// remapping of paameters only if the signal rc-sigbnal conditions are good
} // eof SenderOkay > 140
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// new values from RC
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
{
int tmp_int;
ParameterMapping(); // remapping params (online poti replacement)
if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
{
int tmp_int;
ParameterMapping(); // remapping params (online poti replacement)
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
 
// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
// direct mapping of yaw and thrust
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
 
// direct mapping of yaw and thrust
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
// update max stick positions for pitch, roll and yaw
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
else MaxStickPitch--;
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
else MaxStickRoll--;
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
else MaxStickYaw--;
 
// update max stick positions for pitch, roll and yaw
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
else MaxStickPitch--;
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
else MaxStickRoll--;
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
else MaxStickYaw--;
// update gyro control loop factors
 
// update gyro control loop factors
Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0;
Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000;
 
Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0;
Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000;
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Digital Control via DubWise
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#define KEY_VALUE (FCParam.UserParam1 * 4) // step width
if(DubWiseKeys[1]) BeepTime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE;
else tmp_int = 0;
ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
else tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
#define KEY_VALUE (FCParam.UserParam1 * 4) // step width
if(DubWiseKeys[1]) BeepTime = 10;
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE;
else tmp_int = 0;
ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8;
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE;
else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
else tmp_int = 0;
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
 
if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else
if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0;
if(DubWiseKeys[0] & 2) ExternHeightValue++;
if(DubWiseKeys[0] & 16) ExternHeightValue--;
if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else
if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0;
if(DubWiseKeys[0] & 2) ExternHeightValue++;
if(DubWiseKeys[0] & 16) ExternHeightValue--;
 
StickPitch += ExternStickPitch / 8;
StickRoll += ExternStickRoll / 8;
StickYaw += ExternStickYaw;
StickPitch += ExternStickPitch / 8;
StickRoll += ExternStickRoll / 8;
StickYaw += ExternStickYaw;
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analog control via serial communication
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
{
StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
StickYaw += ExternControl.Yaw;
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
}
// disable I part of gyro control feedback
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0;
// avoid negative scaling factors
if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128)
{
StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
StickYaw += ExternControl.Yaw;
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
}
// disable I part of gyro control feedback
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0;
// avoid negative scaling factors
if(Gyro_P_Factor < 0) Gyro_P_Factor = 0;
if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Looping?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1;
else
{
{
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
else
{
if(Looping_Right) // Hysterese
{
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1;
else
{
{
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
else
{
if(Looping_Right) // Hysterese
{
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
}
}
 
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
else
{
if(Looping_Top) // Hysterese
{
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
else
{
if(Looping_Down) // Hysterese
{
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
else
{
if(Looping_Top) // Hysterese
{
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
}
}
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
else
{
if(Looping_Down) // Hysterese
{
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0;
}
}
 
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0;
} // End of new RC-Values or Emergency Landing
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0;
if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0;
} // End of new RC-Values or Emergency Landing
 
 
if(Looping_Roll) BeepTime = 100;
if(Looping_Roll || Looping_Pitch)
{
if(Looping_Roll) BeepTime = 100;
if(Looping_Roll || Looping_Pitch)
{
if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit;
}
}
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ LED Control on J16/J17
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
LED_OffTime = FCParam.UserParam7;
LED_OnTime = FCParam.UserParam8;
LED_Update();
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// in case of emergency landing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// set all inputs to save values
if(EmergencyLanding)
{
StickYaw = 0;
StickPitch = 0;
StickRoll = 0;
Gyro_P_Factor = 0.5;
Gyro_I_Factor = 0.003;
Looping_Roll = 0;
Looping_Pitch = 0;
MaxStickPitch = 0;
MaxStickRoll = 0;
MaxStickYaw = 0;
}
// set all inputs to save values
if(EmergencyLanding)
{
StickYaw = 0;
StickPitch = 0;
StickRoll = 0;
Gyro_P_Factor = 0.5;
Gyro_I_Factor = 0.003;
Looping_Roll = 0;
Looping_Pitch = 0;
MaxStickPitch = 0;
MaxStickRoll = 0;
MaxStickYaw = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Trim Gyro-Integrals to ACC-Signals
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#define BALANCE_NUMBER 256L
// sum for averaging
MeanIntegralPitch += IntegralPitch;
MeanIntegralRoll += IntegralRoll;
#define BALANCE_NUMBER 256L
// sum for averaging
MeanIntegralPitch += IntegralPitch;
MeanIntegralRoll += IntegralRoll;
 
if(Looping_Pitch || Looping_Roll) // if looping in any direction
{
// reset averaging for acc and gyro integral as well as gyro integral acc correction
MeasurementCounter = 0;
if(Looping_Pitch || Looping_Roll) // if looping in any direction
{
// reset averaging for acc and gyro integral as well as gyro integral acc correction
MeasurementCounter = 0;
 
IntegralAccPitch = 0;
IntegralAccRoll = 0;
IntegralAccPitch = 0;
IntegralAccRoll = 0;
 
MeanIntegralPitch = 0;
MeanIntegralRoll = 0;
MeanIntegralPitch = 0;
MeanIntegralRoll = 0;
 
Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
Reading_IntegralGyroPitch2 = Reading_IntegralGyroPitch;
Reading_IntegralGyroRoll2 = Reading_IntegralGyroRoll;
 
AttitudeCorrectionPitch = 0;
AttitudeCorrectionRoll = 0;
}
AttitudeCorrectionPitch = 0;
AttitudeCorrectionRoll = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction
if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction
{
int32_t tmp_long, tmp_long2;
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
tmp_long /= 16;
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
 
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
{
int32_t tmp_long, tmp_long2;
// determine the deviation of gyro integral from averaged acceleration sensor
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch);
tmp_long /= 16;
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll);
tmp_long2 /= 16;
tmp_long /= 3;
tmp_long2 /= 3;
}
if(MaxStickYaw > 25) // reduce further is yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
}
 
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(MaxStickYaw > 25) // reduce further is yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define BALANCE 32
// limit correction effect
if(tmp_long > BALANCE) tmp_long = BALANCE;
if(tmp_long < -BALANCE) tmp_long =-BALANCE;
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE;
// correct current readings
Reading_IntegralGyroPitch -= tmp_long;
Reading_IntegralGyroRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// MeasurementCounter is incremented in the isr of analog.c
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
{
static int16_t cnt = 0;
static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;
 
#define BALANCE 32
// limit correction effect
if(tmp_long > BALANCE) tmp_long = BALANCE;
if(tmp_long < -BALANCE) tmp_long =-BALANCE;
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE;
if(tmp_long2 <-BALANCE) tmp_long2 =-BALANCE;
// correct current readings
Reading_IntegralGyroPitch -= tmp_long;
Reading_IntegralGyroRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// MeasurementCounter is incremented in the isr of analog.c
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached
// if not lopping in any direction (this should be alwais the case,
// because the Measurement counter is reset to 0 if looping in any direction is active.)
if(!Looping_Pitch && !Looping_Roll)
{
static int16_t cnt = 0;
static int8_t last_n_p, last_n_n, last_r_p, last_r_n;
static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old;
// Calculate mean value of the gyro integrals
MeanIntegralPitch /= BALANCE_NUMBER;
MeanIntegralRoll /= BALANCE_NUMBER;
 
// if not lopping in any direction (this should be alwais the case,
// because the Measurement counter is reset to 0 if looping in any direction is active.)
if(!Looping_Pitch && !Looping_Roll)
{
// Calculate mean value of the gyro integrals
MeanIntegralPitch /= BALANCE_NUMBER;
MeanIntegralRoll /= BALANCE_NUMBER;
// Calculate mean of the acceleration values
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
 
// Calculate mean of the acceleration values
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER;
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER;
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch);
CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim;
AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
 
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorPitch = (int32_t)(MeanIntegralPitch - (int32_t)IntegralAccPitch);
CorrectionPitch = IntegralErrorPitch / ParamSet.GyroAccTrim;
AttitudeCorrectionPitch = CorrectionPitch / BALANCE_NUMBER;
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral
IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
{
AttitudeCorrectionPitch /= 2;
AttitudeCorrectionRoll /= 2;
}
 
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
{
AttitudeCorrectionPitch /= 2;
AttitudeCorrectionRoll /= 2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorPitch = IntegralPitch2 - IntegralPitch;
Reading_IntegralGyroPitch2 -= IntegralErrorPitch;
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gyro-Drift ermitteln
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorPitch = IntegralPitch2 - IntegralPitch;
Reading_IntegralGyroPitch2 -= IntegralErrorPitch;
// deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
 
DebugOut.Analog[17] = IntegralAccPitch / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
DebugOut.Analog[21] = MeanIntegralPitch / 26;
DebugOut.Analog[22] = MeanIntegralRoll / 26;
//DebugOut.Analog[28] = CorrectionPitch;
DebugOut.Analog[29] = CorrectionRoll;
DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
 
DebugOut.Analog[17] = IntegralAccPitch / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
DebugOut.Analog[21] = MeanIntegralPitch / 26;
DebugOut.Analog[22] = MeanIntegralRoll / 26;
//DebugOut.Analog[28] = CorrectionPitch;
DebugOut.Analog[29] = CorrectionRoll;
DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
 
#define ERROR_LIMIT (BALANCE_NUMBER * 4)
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
#define MOVEMENT_LIMIT 20000
// Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionPitch = 0;
if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT)
#define ERROR_LIMIT (BALANCE_NUMBER * 4)
#define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
#define MOVEMENT_LIMIT 20000
// Pitch +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionPitch = 0;
if(labs(MeanIntegralPitch_old - MeanIntegralPitch) < MOVEMENT_LIMIT)
{
if(IntegralErrorPitch > ERROR_LIMIT2)
{
if(IntegralErrorPitch > ERROR_LIMIT2)
if(last_n_p)
{
if(last_n_p)
{
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
CorrectionPitch = IntegralErrorPitch / 8;
if(CorrectionPitch > 5000) CorrectionPitch = 5000;
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
}
else last_n_p = 1;
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
CorrectionPitch = IntegralErrorPitch / 8;
if(CorrectionPitch > 5000) CorrectionPitch = 5000;
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
}
else last_n_p = 0;
if(IntegralErrorPitch < -ERROR_LIMIT2)
else last_n_p = 1;
}
else last_n_p = 0;
if(IntegralErrorPitch < -ERROR_LIMIT2)
{
if(last_n_n)
{
if(last_n_n)
{
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
CorrectionPitch = IntegralErrorPitch / 8;
if(CorrectionPitch < -5000) CorrectionPitch = -5000;
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
}
else last_n_n = 1;
cnt += labs(IntegralErrorPitch) / ERROR_LIMIT2;
CorrectionPitch = IntegralErrorPitch / 8;
if(CorrectionPitch < -5000) CorrectionPitch = -5000;
AttitudeCorrectionPitch += CorrectionPitch / BALANCE_NUMBER;
}
else last_n_n = 0;
else last_n_n = 1;
}
else cnt = 0;
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
// correct Gyro Offsets
if(IntegralErrorPitch > ERROR_LIMIT) AdNeutralPitch += cnt;
if(IntegralErrorPitch < -ERROR_LIMIT) AdNeutralPitch -= cnt;
else last_n_n = 0;
}
else cnt = 0;
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
// correct Gyro Offsets
if(IntegralErrorPitch > ERROR_LIMIT) AdNeutralPitch += cnt;
if(IntegralErrorPitch < -ERROR_LIMIT) AdNeutralPitch -= cnt;
 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionRoll = 0;
if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralErrorPitch) / 4096;
CorrectionRoll = 0;
if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
{
if(IntegralErrorRoll > ERROR_LIMIT2)
{
if(IntegralErrorRoll > ERROR_LIMIT2)
if(last_r_p)
{
if(last_r_p)
{
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
if(CorrectionRoll > 5000) CorrectionRoll = 5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
else last_r_p = 1;
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
if(CorrectionRoll > 5000) CorrectionRoll = 5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
else last_r_p = 0;
if(IntegralErrorRoll < -ERROR_LIMIT2)
else last_r_p = 1;
}
else last_r_p = 0;
if(IntegralErrorRoll < -ERROR_LIMIT2)
{
if(last_r_n)
{
if(last_r_n)
{
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
if(CorrectionRoll < -5000) CorrectionRoll = -5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
else last_r_n = 1;
cnt += labs(IntegralErrorRoll) / ERROR_LIMIT2;
CorrectionRoll = IntegralErrorRoll / 8;
if(CorrectionRoll < -5000) CorrectionRoll = -5000;
AttitudeCorrectionRoll += CorrectionRoll / BALANCE_NUMBER;
}
else last_r_n = 0;
else last_r_n = 1;
}
else cnt = 0;
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;
 
DebugOut.Analog[27] = CorrectionRoll;
DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
else last_r_n = 0;
}
else // looping is active
{
AttitudeCorrectionRoll = 0;
AttitudeCorrectionPitch = 0;
}
else cnt = 0;
// correct Gyro Offsets
if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
if(IntegralErrorRoll > ERROR_LIMIT) AdNeutralRoll += cnt;
if(IntegralErrorRoll < -ERROR_LIMIT) AdNeutralRoll -= cnt;
 
// if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction
if(!Gyro_I_Factor)
{
AttitudeCorrectionRoll = 0;
AttitudeCorrectionPitch = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MeanIntegralPitch_old = MeanIntegralPitch;
MeanIntegralRoll_old = MeanIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
// reset variables used for averaging
IntegralAccPitch = 0;
IntegralAccRoll = 0;
MeanIntegralPitch = 0;
MeanIntegralRoll = 0;
MeasurementCounter = 0;
} // end of averaging
DebugOut.Analog[27] = CorrectionRoll;
DebugOut.Analog[23] = AdNeutralPitch;//10*(AdNeutralPitch - StartNeutralPitch);
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
}
else // looping is active
{
AttitudeCorrectionRoll = 0;
AttitudeCorrectionPitch = 0;
}
 
// if Gyro_I_Faktor == 0 , for example at Heading Hold, ignore attitude correction
if(!Gyro_I_Factor)
{
AttitudeCorrectionRoll = 0;
AttitudeCorrectionPitch = 0;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
MeanIntegralPitch_old = MeanIntegralPitch;
MeanIntegralRoll_old = MeanIntegralRoll;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
// reset variables used for averaging
IntegralAccPitch = 0;
IntegralAccRoll = 0;
MeanIntegralPitch = 0;
MeanIntegralRoll = 0;
MeasurementCounter = 0;
} // end of averaging
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yawing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(MaxStickYaw > 20) // yaw stick is activated
{ // if not fixed compass course is set update compass course
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
}
// exponential stick sensitivity in yawring rate
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx²
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
SetPointYaw = tmp_int;
Reading_IntegralGyroYaw -= tmp_int;
// limit the effect
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
if(MaxStickYaw > 20) // yaw stick is activated
{ // if not fixed compass course is set update compass course
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
}
// exponential stick sensitivity in yawring rate
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx²
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
SetPointYaw = tmp_int;
Reading_IntegralGyroYaw -= tmp_int;
// limit the effect
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Compass
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)
{
int16_t w,v;
static uint8_t updCompass = 0;
 
if (!updCompass--)
{
int16_t w,v;
static uint8_t updCompass = 0;
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
// get current compass heading (angule between MK head and magnetic north)
#ifdef USE_MM3
CompassHeading = MM3_Heading();
#endif
#ifdef USE_CMPS03
CompassHeading = CMPS03_Heading();
#endif
 
if (!updCompass--)
if (CompassHeading < 0) // no compass data available
{
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
// get current compass heading (angule between MK head and magnetic north)
#ifdef USE_MM3
CompassHeading = MM3_Heading();
#endif
#ifdef USE_CMPS03
CompassHeading = CMPS03_Heading();
#endif
 
if (CompassHeading < 0) // no compass data available
{
CompassOffCourse = 0;
if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem
}
else // calculate OffCourse (angular deviation from heading to course)
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
CompassOffCourse = 0;
if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem
}
else // calculate OffCourse (angular deviation from heading to course)
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
}
 
// reduce compass effect with increasing declination
w = abs(IntegralPitch / 512);
v = abs(IntegralRoll / 512);
if(v > w) w = v; // get maximum declination
// if declination is small enough update compass course if neccessary
if(w < 35 && StoreNewCompassCourse)
{
CompassCourse = CompassHeading;
StoreNewCompassCourse = 0;
}
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter
w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
if(w > 0) // if there is any compass effect (avoid negative compass feedback)
{
Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
}
// reduce compass effect with increasing declination
w = abs(IntegralPitch / 512);
v = abs(IntegralRoll / 512);
if(v > w) w = v; // get maximum declination
// if declination is small enough update compass course if neccessary
if(w < 35 && StoreNewCompassCourse)
{
CompassCourse = CompassHeading;
StoreNewCompassCourse = 0;
}
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter
w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
if(w > 0) // if there is any compass effect (avoid negative compass feedback)
{
Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// GPS
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding)
{
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data
}
else
{
GPS_Neutral();
}
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
{
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
if(EmergencyLanding) GPS_Main(230); // enables Comming Home
else GPS_Main(Poti3);
}
else
{
GPS_Neutral();
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!TimerDebugOut--)
{
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor;
DebugOut.Analog[2] = Mean_AccPitch;
DebugOut.Analog[3] = Mean_AccRoll;
DebugOut.Analog[4] = Reading_GyroYaw;
DebugOut.Analog[5] = ReadingHeight;
DebugOut.Analog[6] = (Reading_Integral_Top / 512);
DebugOut.Analog[8] = CompassHeading;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mean_AccTop;
if(!TimerDebugOut--)
{
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz)
DebugOut.Analog[0] = IntegralPitch / ParamSet.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / ParamSet.GyroAccFaktor;
DebugOut.Analog[2] = Mean_AccPitch;
DebugOut.Analog[3] = Mean_AccRoll;
DebugOut.Analog[4] = Reading_GyroYaw;
DebugOut.Analog[5] = ReadingHeight;
DebugOut.Analog[6] = (Reading_Integral_Top / 512);
DebugOut.Analog[8] = CompassHeading;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[16] = Mean_AccTop;
 
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];
DebugOut.Analog[19] = motor_rx[3];
DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
DebugOut.Analog[20] /= 14;
DebugOut.Analog[21] = motor_rx[4];
DebugOut.Analog[22] = motor_rx[5];
DebugOut.Analog[23] = motor_rx[6];
DebugOut.Analog[24] = motor_rx[7];
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 
DebugOut.Analog[9] = Reading_GyroPitch;
DebugOut.Analog[9] = SetPointHeight;
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
DebugOut.Analog[11] = CompassCourse;
DebugOut.Analog[10] = FCParam.Gyro_I;
DebugOut.Analog[10] = ParamSet.Gyro_I;
DebugOut.Analog[9] = CompassOffCourse;
DebugOut.Analog[10] = ThrustMixFraction;
DebugOut.Analog[3] = HeightD * 32;
DebugOut.Analog[4] = HeightControlThrust;
*/
}
DebugOut.Analog[9] = Reading_GyroPitch;
DebugOut.Analog[9] = SetPointHeight;
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
DebugOut.Analog[11] = CompassCourse;
DebugOut.Analog[10] = FCParam.Gyro_I;
DebugOut.Analog[10] = ParamSet.Gyro_I;
DebugOut.Analog[9] = CompassOffCourse;
DebugOut.Analog[10] = ThrustMixFraction;
DebugOut.Analog[3] = HeightD * 32;
DebugOut.Analog[4] = HeightControlThrust;
*/
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor;
else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor;
if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
if(Looping_Pitch) Reading_GyroPitch = Reading_GyroPitch * Gyro_P_Factor;
else Reading_GyroPitch = IntegralPitch * Gyro_I_Factor + Reading_GyroPitch * Gyro_P_Factor;
if(Looping_Roll) Reading_GyroRoll = Reading_GyroRoll * Gyro_P_Factor;
else Reading_GyroRoll = IntegralRoll * Gyro_I_Factor + Reading_GyroRoll * Gyro_P_Factor;
Reading_GyroYaw = Reading_GyroYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2;
 
DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor;
DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor);
DebugOut.Analog[28] = Reading_GyroRoll;
DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor;
DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor);
DebugOut.Analog[28] = Reading_GyroRoll;
 
// limit control feedback
#define MAX_SENSOR 2048
if(Reading_GyroPitch > MAX_SENSOR) Reading_GyroPitch = MAX_SENSOR;
if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR;
if(Reading_GyroRoll > MAX_SENSOR) Reading_GyroRoll = MAX_SENSOR;
if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR;
if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR;
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR;
// limit control feedback
#define MAX_SENSOR 2048
if(Reading_GyroPitch > MAX_SENSOR) Reading_GyroPitch = MAX_SENSOR;
if(Reading_GyroPitch < -MAX_SENSOR) Reading_GyroPitch = -MAX_SENSOR;
if(Reading_GyroRoll > MAX_SENSOR) Reading_GyroRoll = MAX_SENSOR;
if(Reading_GyroRoll < -MAX_SENSOR) Reading_GyroRoll = -MAX_SENSOR;
if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR;
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The higth control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// If hight control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
{
int tmp_int;
// if hight control is activated by an rc channel
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if(FCParam.MaxHeight < 50)
{
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading
HeightControlActive = 0; // disable hight control
}
else HeightControlActive = 1; // enable hight control
}
else // no switchable hight control
// If hight control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
{
int tmp_int;
// if hight control is activated by an rc channel
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if(FCParam.MaxHeight < 50)
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
HeightControlActive = 1;
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading
HeightControlActive = 0; // disable hight control
}
// get current hight
h = ReadingHeight;
// if current hight is above the setpoint reduce thrust
if((h > SetPointHeight) && HeightControlActive)
else HeightControlActive = 1; // enable hight control
}
else // no switchable hight control
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
HeightControlActive = 1;
}
// get current hight
h = ReadingHeight;
// if current hight is above the setpoint reduce thrust
if((h > SetPointHeight) && HeightControlActive)
{
// hight difference -> P control part
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
h = ThrustMixFraction - h; // reduce gas
// higth gradient --> D control part
h -= (HeightD * FCParam.Height_D) / 8; // D control part
// acceleration sensor effect
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
if(tmp_int > 50) tmp_int = 50;
if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
// update hight control thrust
HeightControlThrust = (HeightControlThrust*15 + h) / 16;
// limit thrust reduction
if(HeightControlThrust < ParamSet.Height_MinThrust)
{
// hight difference -> P control part
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
h = ThrustMixFraction - h; // reduce gas
// higth gradient --> D control part
h -= (HeightD * FCParam.Height_D) / 8; // D control part
// acceleration sensor effect
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
if(tmp_int > 50) tmp_int = 50;
if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
// update hight control thrust
HeightControlThrust = (HeightControlThrust*15 + h) / 16;
// limit thrust reduction
if(HeightControlThrust < ParamSet.Height_MinThrust)
{
if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
// allows landing also if thrust stick is reduced below min thrust on hight control
if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
}
// limit thrust to stick setting
if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;
ThrustMixFraction = HeightControlThrust;
if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
// allows landing also if thrust stick is reduced below min thrust on hight control
if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
}
// limit thrust to stick setting
if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;
ThrustMixFraction = HeightControlThrust;
}
// limit thrust to parameter setting
if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20;
}
// limit thrust to parameter setting
if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Mixer and PI-Controller
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[7] = ThrustMixFraction;
DebugOut.Analog[7] = ThrustMixFraction;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yaw-Fraction
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/branches/V0.68d Code Redesign killagreg/fc.h
74,7 → 74,7
void Beep(uint8_t numbeeps);
 
 
extern int16_t Poti1, Poti2, Poti3, Poti4;
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
/branches/V0.68d Code Redesign killagreg/gps.h
9,8 → 9,9
extern uint8_t GPS_P_Factor;
extern uint8_t GPS_D_Factor;
 
extern void GPS_Main(void);
extern void GPS_Main(uint8_t ctrl);
extern void GPS_SetHomePosition(void);
extern void GPS_ClearHomePosition(void);
extern void GPS_Neutral(void);
 
#endif //_GPS_H
/branches/V0.68d Code Redesign killagreg/main.c
61,6 → 61,7
#if defined (__AVR_ATmega644P__)
#include "uart1.h"
#endif
#include "led.h"
#include "menu.h"
#include "ubx.h"
#include "fc.h"
105,12 → 106,7
ROT_ON;
GRN_OFF;
 
// set PC2 & PC3 as output (ctrl J16 & J17)
DDRC |= (1<<DDC2)|(1<<DDC3);
J16_OFF;
J17_OFF;
 
// disable watchdog
// disable watchdog
MCUSR &=~(1<<WDRF);
WDTCSR |= (1<<WDCE)|(1<<WDE);
WDTCSR = 0;
125,6 → 121,7
ROT_OFF;
 
// initalize modules
LED_Init();
TIMER0_Init();
TIMER2_Init();
USART0_Init();
/branches/V0.68d Code Redesign killagreg/main.h
1,6 → 1,8
#ifndef _MAIN_H
#define _MAIN_H
#define _MAIN_H
 
#include <avr/io.h>
 
//Hier die Quarz Frequenz einstellen
#if defined (__AVR_ATmega32__)
#define SYSCLK 20000000L //Quarz Frequenz in Hz
25,11 → 27,6
#define GRN_ON PORTB |= (1<<PORTB1)
#define GRN_FLASH PORTB ^= (1<<PORTB1)
 
#define J16_ON PORTC |= (1<<PORTC2)
#define J16_OFF PORTC &= ~(1<<PORTC2)
#define J17_ON PORTC |= (1<<PORTC3)
#define J17_OFF PORTC &= ~(1<<PORTC3)
 
#include <inttypes.h>
 
extern uint8_t BoardRelease;
/branches/V0.68d Code Redesign killagreg/makefile
80,7 → 80,7
 
##########################################################################################################
# List C source files here. (C dependencies are automatically generated.)
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c
SRC = main.c uart.c printf_P.c timer0.c timer2.c analog.c menu.c led.c
SRC += twimaster.c rc.c fc.c GPS.c spi.c eeprom.c mymath.c ubx.c fifo.c
ifeq ($(MCU), atmega644p)
SRC += uart1.c
/branches/V0.68d Code Redesign killagreg/menu.c
161,10 → 161,10
LCD_printfxy(0,3,"OffCourse: %5i",CompassOffCourse);
break;
case 9:// Poti Menu Item
LCD_printfxy(0,0,"Poti1: %3i",Poti1);
LCD_printfxy(0,1,"Poti2: %3i",Poti2);
LCD_printfxy(0,2,"Poti3: %3i",Poti3);
LCD_printfxy(0,3,"Poti4: %3i",Poti4);
LCD_printfxy(0,0,"Po1: %3i Po5: %3i" ,Poti1,Poti5); //PPM24-Extesion
LCD_printfxy(0,1,"Po2: %3i Po6: %3i" ,Poti2,Poti6); //PPM24-Extesion
LCD_printfxy(0,2,"Po3: %3i Po7: %3i" ,Poti3,Poti7); //PPM24-Extesion
LCD_printfxy(0,3,"Po4: %3i Po8: %3i" ,Poti4,Poti8); //PPM24-Extesion
break;
case 10:// Servo Menu Item
LCD_printfxy(0,0,"Servo " );
/branches/V0.68d Code Redesign killagreg/rc.c
114,7 → 114,7
{
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if(index >= 4) NewPpmData = 0; // Null means NewData
if(index >= 4) NewPpmData = 0; // Null means NewData for the first 4 channels
// synchronize channel index
index = 1;
}
/branches/V0.68d Code Redesign killagreg/uart.c
56,7 → 56,7
"AccPitch ",
"AccRoll ",
"GyroYaw ",
"ReadingHeight ", //5
"ReadingHeight ", //5
"AccZ ",
"Thrust ",
"CompassHeading ",