66,7 → 66,7 |
#include "twimaster.h" |
#include "mm3.h" |
|
volatile unsigned int I2CTimeout = 100; |
volatile uint16_t I2CTimeout = 100; |
// gyro readings |
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
// gyro neutral readings |
90,7 → 90,7 |
volatile int32_t MeanIntegralRoll; |
|
// attitude acceleration integrals |
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0, IntegralAccZ = 0; |
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0; |
volatile int32_t Reading_Integral_Top = 0; |
|
// compass course |
177,7 → 177,7 |
AdNeutralYaw = AdValueGyrYaw; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralPitch = AdNeutralPitch; |
if(GetParamByte(PID_ACC_PITCH) > 4) |
if(GetParamWord(PID_ACC_PITCH) > 1023) |
{ |
NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY; |
232,7 → 232,6 |
// sum sensor readings for later averaging |
IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch; |
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
IntegralAccZ += Current_AccZ - NeutralAccZ; |
|
// Yaw |
// calculate yaw gyro intergral (~ to rotation angle) |
350,9 → 349,10 |
Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch; |
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
Mean_AccTop = (int32_t)AdValueAccTop; |
// start ADC |
// start ADC (enables internal trigger so that the ISR in analog.c |
// updates the readings once) |
ADC_Enable(); |
//update poti values by rc-signals (why not +127?) |
//update poti values by rc-signals |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
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--; |
364,7 → 364,7 |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
|
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
} |
|
/************************************************************************/ |
379,8 → 379,8 |
Motor_Right = 0; |
Motor_Left = 0; |
if(MotorTest[0]) Motor_Front = MotorTest[0]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
} |
|
684,7 → 684,7 |
StickYaw += ExternStickYaw; |
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Control via serial communication |
//+ Analog control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
783,7 → 783,6 |
|
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
|
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
805,12 → 804,12 |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long2 /= 16; |
|
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(MaxStickYaw > 25) |
if(MaxStickYaw > 25) // reduce further is yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
817,7 → 816,7 |
} |
|
#define BALANCE 32 |
// limit correction |
// limit correction effect |
if(tmp_long > BALANCE) tmp_long = BALANCE; |
if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
830,9 → 829,9 |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int cnt = 0; |
static char last_n_p, last_n_n, last_r_p, last_r_n; |
static long MeanIntegralPitch_old, MeanIntegralRoll_old; |
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; |
|
// 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.) |
845,7 → 844,6 |
// Calculate mean of the acceleration values |
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER; |
IntegralAccZ = IntegralAccZ / BALANCE_NUMBER; |
|
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
983,7 → 981,6 |
// reset variables used for averaging |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeasurementCounter = 0; |
1039,7 → 1036,7 |
StoreNewCompassCourse = 0; |
} |
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination |
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; |
1129,7 → 1126,7 |
// Hight Control |
// The higth control algorithm reduces the thrust but does not increase the thrust. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// If hight control is activated and no emergency landing is activre |
// If hight control is activated and no emergency landing is active |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
{ |
int tmp_int; |