| 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; |